From 18b76d17dbf93f516129d5937456751b516b532e Mon Sep 17 00:00:00 2001 From: Robert Pelnar Date: Mon, 26 Mar 2018 14:11:15 +0200 Subject: [PATCH 1/3] New XYZ calibration with image processing --- Firmware/Configuration_prusa.h | 2 +- Firmware/Dcodes.cpp | 163 +++++++ Firmware/Dcodes.h | 3 + Firmware/Marlin_main.cpp | 27 +- Firmware/mesh_bed_calibration.cpp | 64 ++- Firmware/sm4.c | 180 +++++++ Firmware/sm4.h | 45 ++ Firmware/temperature.h | 4 + Firmware/xyzcal.cpp | 782 ++++++++++++++++++++++++++++++ Firmware/xyzcal.h | 47 ++ 10 files changed, 1298 insertions(+), 19 deletions(-) create mode 100644 Firmware/sm4.c create mode 100644 Firmware/sm4.h create mode 100644 Firmware/xyzcal.cpp create mode 100644 Firmware/xyzcal.h diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h index 47939e07a..26ce09286 100644 --- a/Firmware/Configuration_prusa.h +++ b/Firmware/Configuration_prusa.h @@ -172,7 +172,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o #define FANCHECK //#define WATCHDOG //#define SAFETYTIMER - +#define NEW_XYZCAL /*------------------------------------ LOAD/UNLOAD FILAMENT SETTINGS diff --git a/Firmware/Dcodes.cpp b/Firmware/Dcodes.cpp index 7f7681739..cc9a698aa 100644 --- a/Firmware/Dcodes.cpp +++ b/Firmware/Dcodes.cpp @@ -470,6 +470,169 @@ void dcode_12() eeprom_update_byte((uint8_t*)EEPROM_POWER_COUNT, 0x00); } +#include "xyzcal.h" +void sync_command_P(const char* cmd, bool wait_movements) +{ + enquecommand_front_P(cmd); + process_commands(); + cmdqueue_pop_front(); + if (wait_movements) + st_synchronize(); +} + +#include "tmc2130.h" + +extern long count_position[NUM_AXIS]; + +void dcode_15() +{//PINDA scan + int p = -1; + if (code_seen('P')) + { + printf_P(PSTR("code seen P\n")); + p = code_value(); + switch (p) + { + case 0: enquecommand_front_P((PSTR("G1 X12 Y6 F6000"))); break; + case 1: enquecommand_front_P((PSTR("G1 X220 Y6 F6000"))); break; + case 2: enquecommand_front_P((PSTR("G1 X220 Y198 F6000"))); break; + case 3: enquecommand_front_P((PSTR("G1 X12 Y198 F6000"))); break; + } + return; + } + if (code_seen('H')) + {// meassure pinda hysterezis + printf_P(PSTR("code seen H\n")); + st_synchronize(); + if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS])) + { + sync_command_P(PSTR("G28W"), true); + } + sync_command_P(PSTR("G1 X12 Y6 Z2"), true); + xyzcal_meassure_enter(); + uint16_t pinda_hysterezis = xyzcal_meassure_pinda_hysterezis(-80, 5000, 500, 5); + xyzcal_meassure_leave(); + printf_P(PSTR("pinda_hysterezis = %d\n"), pinda_hysterezis); + return; + } + if (code_seen('U')) + {// Z up while pinda on + printf_P(PSTR("code seen U\n")); + uint16_t steps = xyzcal_stepZ_up_while_on(5000, 500); + printf_P(PSTR(" steps = %d\n"), steps); + return; + } + if (code_seen('B')) + {// Z down while pinda off + printf_P(PSTR("code seen B\n")); + uint16_t steps = xyzcal_stepZ_dn_while_off(-80, 500); + printf_P(PSTR(" steps = %d\n"), steps); + return; + } + if (code_seen('X')) + {// print counter positions + printf_P(PSTR("code seen X\n")); + printf_P(PSTR("X=%d Y=%d Z=%d\n"), (int16_t)count_position[X_AXIS], (int16_t)count_position[Y_AXIS], (int16_t)count_position[Z_AXIS]); + return; + } + if (code_seen('Y')) + {// print counter positions + printf_P(PSTR("code seen X\n")); + printf_P(PSTR("X=%ld Y=%ld Z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); + return; + } + if (code_seen('O')) + {// print counter positions + printf_P(PSTR("code seen O\n")); + xyzcal_meassure_enter(); + int16_t x = (int16_t)count_position[X_AXIS]; + int16_t y = (int16_t)count_position[Y_AXIS]; + int16_t z = (int16_t)count_position[Z_AXIS]; + xyzcal_find_point_center(x, y, z, z-10, z+10, 500, 10); + xyzcal_meassure_leave(); +// printf_P(PSTR("X=%d Y=%d Z=%d\n"), (int16_t)count_position[X_AXIS], (int16_t)count_position[Y_AXIS], (int16_t)count_position[Z_AXIS]); + return; + } + if (code_seen('S')) + {// print counter positions + printf_P(PSTR("code seen S\n")); + int16_t cx = (int16_t)count_position[X_AXIS]; + int16_t cy = (int16_t)count_position[Y_AXIS]; + int16_t z = (int16_t)count_position[Z_AXIS]; + xyzcal_meassure_enter(); + xyzcal_scan_pixels_32x32(cx, cy, z-96, z+383, 200, 0); + xyzcal_meassure_leave(); +// printf_P(PSTR("X=%d Y=%d Z=%d\n"), (int16_t)count_position[X_AXIS], (int16_t)count_position[Y_AXIS], (int16_t)count_position[Z_AXIS]); + return; + } + + if (code_seen('L')) + {// print counter positions + printf_P(PSTR("code seen L\n")); + int16_t x = (int16_t)count_position[X_AXIS]; + int16_t y = (int16_t)count_position[Y_AXIS]; + int16_t z = (int16_t)count_position[Z_AXIS]; + if (code_seen('x')) x = code_value(); + if (code_seen('y')) y = code_value(); + if (code_seen('z')) z = code_value(); + xyzcal_meassure_enter(); + xyzcal_lineXYZ_to(x, y, z, 320, 0); + xyzcal_meassure_leave(); +// printf_P(PSTR("X=%d Y=%d Z=%d\n"), (int16_t)count_position[X_AXIS], (int16_t)count_position[Y_AXIS], (int16_t)count_position[Z_AXIS]); + return; + } + printf_P(PSTR("no code seen \n")); + return; +/* + xyzcal_meassure_enter(); + + tmc2130_set_dir(X_AXIS, 0); + + int z = 0; + + int8_t _pinda = xyzcal_read_pinda(); + for (int x = 0; x < 10000; x++) + { + int8_t pinda = xyzcal_read_pinda(); + if (_pinda != pinda) + { + if (pinda) + printf_P(PSTR("!1 x=%d z=%d\n"), x, z+23); + else + printf_P(PSTR("!0 x=%d z=%d\n"), x, z-24); + _pinda = pinda; + } + tmc2130_set_dir(Z_AXIS, !pinda); + if (!pinda) + { + if (z > 0) + { + tmc2130_do_step(Z_AXIS); + z--; + } + } + else + { + tmc2130_do_step(Z_AXIS); + z++; + } + tmc2130_do_step(X_AXIS); + delayMicroseconds(400); + } + xyzcal_meassure_leave(); +return; +*/ +} + +void dcode_16() +{ + xyzcal_find_bed_induction_sensor_point_xy(); + return; +// xyzcal_meassure_enter(); +// xyzcal_searchZ(); +// xyzcal_meassure_leave(); +} + #ifdef TMC2130 #include "planner.h" diff --git a/Firmware/Dcodes.h b/Firmware/Dcodes.h index 69a55b13b..081619045 100644 --- a/Firmware/Dcodes.h +++ b/Firmware/Dcodes.h @@ -17,6 +17,9 @@ extern void dcode_9(); //D9 - Read/Write ADC (Write=enable simulated, Read=disab extern void dcode_10(); //D10 - XYZ calibration = OK extern void dcode_12(); //D12 - Reset failstat counters +extern void dcode_15(); //D15 +extern void dcode_16(); //D16 + #ifdef TMC2130 extern void dcode_2130(); //D2130 - TMC2130 #endif //TMC2130 diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 102482401..d5078875e 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -1306,7 +1306,7 @@ void setup() KEEPALIVE_STATE(NOT_BUSY); #ifdef WATCHDOG - wdt_enable(WDTO_4S); + wdt_enable(WDTO_4S); #endif //WATCHDOG } @@ -2228,7 +2228,7 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level) 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) { #ifdef HEATBED_V2 @@ -2251,6 +2251,7 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level) // if (result >= 0) babystep_apply(); #endif //HEATBED_V2 } +#endif //NEW_XYZCAL lcd_bed_calibration_show_result(result, point_too_far_mask); if (result >= 0) @@ -3228,6 +3229,16 @@ void process_commands() #ifdef PINDA_THERMISTOR if (true) { + 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; + 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); if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS])) { // We don't know where we are! HOME! // Push the commands to the front of the message queue in the reverse order! @@ -3236,10 +3247,6 @@ 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_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"); @@ -6406,6 +6413,12 @@ Sigma_Exit: case 12: //D12 - Reset failstat counters dcode_12(); break; + case 15: //D15 - + dcode_15(); break; + + case 16: //D16 - + dcode_16(); break; + #ifdef TMC2130 case 2130: // D9125 - TMC2130 dcode_2130(); break; @@ -6902,7 +6915,7 @@ void kill(const char *full_screen_message, unsigned char id) while(1) { #ifdef WATCHDOG - wdt_reset(); + wdt_reset(); #endif //WATCHDOG /* Intentionally left empty */ diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp index 8db94cd42..973956543 100644 --- a/Firmware/mesh_bed_calibration.cpp +++ b/Firmware/mesh_bed_calibration.cpp @@ -19,8 +19,11 @@ float world2machine_shift[2]; #define WEIGHT_FIRST_ROW_Y_HIGH (0.3f) #define WEIGHT_FIRST_ROW_Y_LOW (0.0f) +//#define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER) // -22 + 23 = 1 +//#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER) // -0.6 + 5 = 4.4 + #define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER) // -22 + 23 = 1 -#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER) // -0.6 + 5 = 4.4 +#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER + 4) // -0.6 + 5 = 4.4 // Scaling of the real machine axes against the programmed dimensions in the firmware. // The correction is tiny, here around 0.5mm on 250mm length. @@ -56,10 +59,10 @@ const float bed_skew_angle_extreme = (0.25f * M_PI / 180.f); // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor. // The points are the following: center front, center right, center rear, center left. const float bed_ref_points_4[] PROGMEM = { - 13.f - BED_ZERO_REF_X, 10.4f - 4.f - BED_ZERO_REF_Y, - 221.f - BED_ZERO_REF_X, 10.4f - 4.f - BED_ZERO_REF_Y, - 221.f - BED_ZERO_REF_X, 202.4f - 4.f - BED_ZERO_REF_Y, - 13.f - BED_ZERO_REF_X, 202.4f - 4.f - BED_ZERO_REF_Y + 13.f - BED_ZERO_REF_X, 10.4f - BED_ZERO_REF_Y, + 221.f - BED_ZERO_REF_X, 10.4f - BED_ZERO_REF_Y, + 221.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y, + 13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y }; const float bed_ref_points[] PROGMEM = { @@ -905,6 +908,10 @@ 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. @@ -918,9 +925,13 @@ error: #define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f) #endif //HEATBED_V2 + #ifdef HEATBED_V2 -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 @@ -1163,8 +1174,9 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level) enable_z_endstop(false); invert_z_endstop(false); return found; - +#endif //NEW_XYZCAL } + #else //HEATBED_V2 inline bool find_bed_induction_sensor_point_xy(int verbosity_level) { @@ -1364,11 +1376,17 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level) #endif //HEATBED_V2 +#ifdef NEW_XYZCAL +extern bool xyzcal_improve_bed_induction_sensor_point(void); +#endif //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. -inline bool improve_bed_induction_sensor_point() +/*inline */bool improve_bed_induction_sensor_point() { +#ifdef NEW_XYZCAL + return xyzcal_improve_bed_induction_sensor_point(); +#else //NEW_XYZCAL static const float search_radius = 8.f; bool endstops_enabled = enable_endstops(false); @@ -1452,6 +1470,7 @@ inline bool improve_bed_induction_sensor_point() enable_endstops(endstops_enabled); enable_z_endstop(endstop_z_enabled); return found; +#endif //NEW_XYZCAL } static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z) @@ -1467,12 +1486,19 @@ static inline void debug_output_point(const char *type, const float &x, const fl SERIAL_ECHOLNPGM(""); } +#ifdef NEW_XYZCAL +extern bool xyzcal_improve_bed_induction_sensor_point2(bool lift_z_on_min_y); +#endif //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. #define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f) -inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level) +/*inline */bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level) { +#ifdef NEW_XYZCAL + return xyzcal_improve_bed_induction_sensor_point(); +#else //NEW_XYZCAL + float center_old_x = current_position[X_AXIS]; float center_old_y = current_position[Y_AXIS]; float a, b; @@ -1625,16 +1651,23 @@ canceled: enable_z_endstop(false); go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); return false; +#endif //NEW_XYZCAL } +#ifdef NEW_XYZCAL +extern bool xyzcal_improve_bed_induction_sensor_point3(void); +#endif //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. // If this function failed, the Y coordinate will never be outside the working space. #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (8.f) #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f) -inline bool improve_bed_induction_sensor_point3(int verbosity_level) +/*inline */bool improve_bed_induction_sensor_point3(int verbosity_level) { +#ifdef NEW_XYZCAL + return xyzcal_improve_bed_induction_sensor_point3(); +#else //NEW_XYZCAL float center_old_x = current_position[X_AXIS]; float center_old_y = current_position[Y_AXIS]; float a, b; @@ -1946,8 +1979,10 @@ canceled: current_position[Y_AXIS] = Y_MIN_POS; 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. @@ -1992,6 +2027,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 @@ -2379,7 +2415,11 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8 current_position[Z_AXIS] -= 0.025f; // Improve the point position by searching its center in a current plane. int8_t n_errors = 3; - for (int8_t iter = 0; iter < 8; ) { +#ifdef NEW_XYZCAL + for (int8_t iter = 0; iter < 1; ) { +#else //NEW_XYZCAL + for (int8_t iter = 0; iter < 8; ) { +#endif //NEW_XYZCAL #ifdef SUPPORT_VERBOSITY if (verbosity_level > 20) { SERIAL_ECHOPGM("Improving bed point "); @@ -2732,6 +2772,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. @@ -2793,6 +2834,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/sm4.c b/Firmware/sm4.c new file mode 100644 index 000000000..1168abf28 --- /dev/null +++ b/Firmware/sm4.c @@ -0,0 +1,180 @@ +//sm4.c - simple 4-axis stepper control + +#include "sm4.h" +#include + +#include "boards.h" +#define bool int8_t +#define false 0 +#define true 1 +#include "Configuration_prusa.h" + +/**/ + +//direction signal pinout - 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 pinout - 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_update_pos_cb sm4_update_pos = 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)) +} + +int isqrt(int n) +{ + int a = 1; + int b = n; + while (abs(a - b) > 1) + { + b = n / a; + a = (a + b) / 2; + } + return a; +} + +uint8_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t delay_us, sm4_stop_cb stop_cb) +{ + 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; + uint8_t stop = 0; + while ((nd--) && stop_cb && !(stop = (*stop_cb)())) + { + 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); + delayMicroseconds(delay_us); + } + if (sm4_update_pos) (*sm4_update_pos)(x, y, z, 0); + return stop; +} + diff --git a/Firmware/sm4.h b/Firmware/sm4.h new file mode 100644 index 000000000..46416b4ab --- /dev/null +++ b/Firmware/sm4.h @@ -0,0 +1,45 @@ +//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)(); + +// callback prototype for updating position counters +typedef void (*sm4_update_pos_cb)(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de); + +// callback pointer +extern sm4_update_pos_cb sm4_update_pos; + +// returns direction for single axis +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 +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 msk); + +// step axes by bitmask +extern void sm4_do_step(uint8_t axes_mask); + +// xyz linear-interpolated relative move +uint8_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t delay_us, sm4_stop_cb stop); + + +#if defined(__cplusplus) +} +#endif //defined(__cplusplus) +#endif //_SM4_H diff --git a/Firmware/temperature.h b/Firmware/temperature.h index 91791c4cf..ec5670516 100644 --- a/Firmware/temperature.h +++ b/Firmware/temperature.h @@ -27,6 +27,10 @@ #include "stepper.h" #endif +#define ENABLE_TEMPERATURE_INTERRUPT() TIMSK0 |= (1< +#include "stepper.h" +#include "temperature.h" +#include "sm4.h" +//#include "tmc2130.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 _PI 3.14159265F + +extern long count_position[NUM_AXIS]; + +void xyzcal_meassure_enter(void) +{ + printf_P(PSTR("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 +} + +void xyzcal_meassure_leave(void) +{ + printf_P(PSTR("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 +} + +int8_t xyzcal_read_pinda(void) +{ + return ((READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)?1:0); +} + +uint16_t xyzcal_stepZ_up_while_on(int16_t max_z, uint16_t delay_us) +{ +// printf_P(PSTR("xyzcal_stepZ_up_while_on %d\n"), max_z); + if (!xyzcal_read_pinda()) return 0; + uint16_t steps = 0; + sm4_set_dir(Z_AXIS, 0); + while (xyzcal_read_pinda() && (count_position[Z_AXIS] < (long)max_z)) + { + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us); + count_position[Z_AXIS]++; + steps++; + } + return steps; +} + +uint16_t xyzcal_stepZ_dn_while_off(int16_t min_z, uint16_t delay_us) +{ +// printf_P(PSTR("xyzcal_stepZ_dn_while_off %d\n"), min_z); + if (xyzcal_read_pinda()) return 0; + uint16_t steps = 0; + sm4_set_dir(Z_AXIS, 1); + while (!xyzcal_read_pinda() && (count_position[Z_AXIS] > (long)min_z)) + { + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us); + count_position[Z_AXIS]--; + steps++; + } + return steps; +} + +void xyzcal_stepZ_by(int16_t z_delta, uint16_t delay_us) +{ + sm4_set_dir(Z_AXIS, (z_delta < 0)?1:0); + while (z_delta) + { + sm4_do_step(Z_AXIS_MASK); + delayMicroseconds(delay_us); + if (z_delta > 0) + { + count_position[Z_AXIS]++; + z_delta--; + } + else + { + count_position[Z_AXIS]--; + z_delta++; + } + } +} + +void xyzcal_stepZ_to(int16_t z_target, uint16_t delay_us) +{ +// printf_P(PSTR("xyzcal_stepZ_to %d\n"), z_target); + xyzcal_stepZ_by(z_target - count_position[Z_AXIS], delay_us); +} + +bool xyzcal_lineXYZ_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t delay_us, int8_t check_pinda) +{ +// printf_P(PSTR("xyzcal_lineXYZ_ui %u %u %u %u %d\n"), dx, dy, dz, delay_us, check_pinda); + uint16_t d = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz))) + 0.5); +// printf_P(PSTR(" d = %u\n"), d); + uint16_t cx = d; + uint16_t cy = d; + uint16_t cz = d; + uint16_t nx = dx; + uint16_t ny = dy; + uint16_t nz = dz; + uint8_t msk = 0; +// int8_t pinda = 0; + int8_t pinda = xyzcal_read_pinda(); +// printf_P(PSTR(" pinda = %d\n"), pinda); + while (nx || ny || nz) + { + msk = 0; + if (cx <= dx) + { + msk |= 1; + nx--; + cx += d; + } + if (cy <= dy) + { + msk |= 2; + ny--; + cy += d; + } + if (cz <= dz) + { + msk |= 4; + nz--; + cz += d; + } + cx -= dx; + cy -= dy; + cz -= dz; + sm4_do_step(msk); + delayMicroseconds(delay_us); + if (check_pinda) + { + pinda = xyzcal_read_pinda(); + if ((check_pinda > 0) && pinda) break; + if ((check_pinda < 0) && !pinda) break; + } +// printf_P(PSTR("%d %d %d %d\n"), nx, ny, nz, delay_us); + } + if (sm4_get_dir(X_AXIS)) count_position[X_AXIS] -= (dx - nx); + else count_position[X_AXIS] += (dx - nx); + if (sm4_get_dir(Y_AXIS)) count_position[Y_AXIS] -= (dy - ny); + else count_position[Y_AXIS] += (dy - ny); + if (sm4_get_dir(Z_AXIS)) count_position[Z_AXIS] -= (dz - nz); + else count_position[Z_AXIS] += (dz - nz); + if ((check_pinda > 0) && pinda) + { +// int8_t pinda = xyzcal_read_pinda(); +// printf_P(PSTR(" pinda = %d\n"), pinda); +// printf_P(PSTR("PINDA 0>1\n")); + return true; + } + if ((check_pinda < 0) && !pinda) + { +// int8_t pinda = xyzcal_read_pinda(); +// printf_P(PSTR(" pinda = %d\n"), pinda); +// printf_P(PSTR("PINDA 1>0\n")); + return true; + } + return false; +} + +uint8_t check_pinda_cb_0() +{ + return xyzcal_read_pinda()?0:1; +} + +uint8_t check_pinda_cb_1() +{ + return xyzcal_read_pinda()?1:0; +} + +uint8_t xyzcal_dm = 0; + +void xyzcal_update_pos_cb(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de) +{ + if (dx) count_position[0] += (xyzcal_dm&1)?-dx:dx; + if (dy) count_position[1] += (xyzcal_dm&2)?-dy:dy; + if (dz) count_position[2] += (xyzcal_dm&4)?-dz:dz; +} + +bool xyzcal_lineXYZ(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t 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) | ((z<0)?2:0) | ((y<0)?4:0); + sm4_set_dir_bits(xyzcal_dm); + sm4_line_xyz_ui(x, y, z, delay_us, check_pinda?((check_pinda<0)?check_pinda_cb_0:check_pinda_cb_1):0)?true:false; +} + +bool xyzcal_lineXYZ_by(int16_t dx, int16_t dy, int16_t dz, uint16_t delay_us, int8_t check_pinda) +{ + if (dx > 0) sm4_set_dir(X_AXIS, 0); + else if (dx < 0) sm4_set_dir(X_AXIS, 1); + if (dy > 0) sm4_set_dir(Y_AXIS, 0); + else if (dy < 0) sm4_set_dir(Y_AXIS, 1); + if (dz > 0) sm4_set_dir(Z_AXIS, 0); + else if (dz < 0) sm4_set_dir(Z_AXIS, 1); + return xyzcal_lineXYZ_ui(abs(dx), abs(dy), abs(dz), delay_us, check_pinda); +} + +bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda) +{ +// printf_P(PSTR(" xyzcal_lineXYZ_to %d %d %d %u\n"), x, y, z, delay_us); + return xyzcal_lineXYZ_by(x - (int16_t)count_position[0], y - (int16_t)count_position[1], z - (int16_t)count_position[2], 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; + 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; + printf_P(PSTR("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; + printf_P(PSTR("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; +} + +int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples) +{ +// printf_P(PSTR("xyzcal_meassure_pinda_hysterezis\n")); + int8_t ret = -1; // PINDA signal error + int16_t z = (int16_t)count_position[Z_AXIS]; + int16_t sum_up = 0; + int16_t sum_dn = 0; + int16_t up; + int16_t dn; + uint8_t sample; + dn = xyzcal_stepZ_dn_while_off(min_z, delay_us); +// printf_P(PSTR("dn=%d\n"), sample, up, dn); + up = xyzcal_stepZ_up_while_on(max_z, delay_us); +// printf_P(PSTR("up=%d\n"), sample, up, dn); + if (!xyzcal_read_pinda()) + { + for (sample = 0; sample < samples; sample++) + { + dn = xyzcal_stepZ_dn_while_off(min_z, 2*delay_us); + if (!xyzcal_read_pinda()) break; + up = xyzcal_stepZ_up_while_on(max_z, 2*delay_us); + if (xyzcal_read_pinda()) break; +// printf_P(PSTR("%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_stepZ_to(z, delay_us); //return to original Z position + return ret; +} + +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) +{ + printf_P(PSTR("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 = xyzcal_read_pinda(); + xyzcal_lineXYZ_to((r&1)?(cx+1024):(cx-1024), cy - 1024 + r*64, z, 2*delay_us, 0); + xyzcal_stepZ_dn_while_off(min_z, 2*delay_us); + xyzcal_stepZ_up_while_on(max_z, 2*delay_us); + 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 = xyzcal_read_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) +// printf_P(PSTR("!1 x=%d z=%d\n"), c*64+i, z+23); +// else +// printf_P(PSTR("!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; +// printf_P(PSTR("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++) + printf_P(PSTR("%02x"), pixels[((uint16_t)r<<5) + c]); + printf_P(PSTR("\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++) + printf_P(PSTR(" %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; + } + } + printf_P(PSTR("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; + printf_P(PSTR(" 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++) + printf_P(PSTR("%02x"), pixels[((uint16_t)r<<5) + c]); + printf_P(PSTR("\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; + } +// printf("%2d %2d %d\n", x, y, match8x8(0, pixels, w, h, x, y)); + } + printf("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; +} + +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 = xyzcal_read_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 = xyzcal_read_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) + printf_P(PSTR("!1 x=%d y=%d\n"), x, y); + else + printf_P(PSTR("!0 x=%d y=%d\n"), x, y); + }*/ + _pinda = pinda; +// printf_P(PSTR("x=%d y=%d rx=%d ry=%d\n"), x, y, rx, ry); + } + printf_P(PSTR("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; + printf_P(PSTR("x0=%d y0=%d r=%d\n"), x0, y0, r); + } + } + xyzcal_lineXYZ_to(x0, y0, z, 200, 0); +} + +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; +} + +//const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200}; +//const int16_t PROGMEM xyzcal_point_ycoords[4] = {600, 600, 19800, 19800}; +const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200}; +const int16_t PROGMEM xyzcal_point_ycoords[4] = {700, 700, 19800, 19800}; + +const int16_t PROGMEM xyzcal_point_xcoords_[4] = {1131, 21939, 21964, 1122}; +const int16_t PROGMEM xyzcal_point_ycoords_[4] = {709, 674, 19883, 19922}; + +const uint16_t PROGMEM xyzcal_point_pattern[12] = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000}; +/* +int16_t xyzcal_point2xcoord(uint8_t point) +{ + return xyzcal_point_xcoords[point & 3]; +} + +int16_t xyzcal_point2ycoord(uint8_t point) +{ + return xyzcal_point_ycoords[point & 3]; +} +*/ +bool xyzcal_searchZ(void) +{ + printf_P(PSTR("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); + int16_t x0 = (int16_t)count_position[X_AXIS]; + int16_t y0 = (int16_t)count_position[Y_AXIS]; + int16_t z0 = (int16_t)count_position[Z_AXIS]; +// 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 = (int16_t)count_position[X_AXIS]; + int16_t y_on = (int16_t)count_position[Y_AXIS]; + int16_t z_on = (int16_t)count_position[Z_AXIS]; + printf_P(PSTR(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), x_on, y_on, z_on, ad); + return true; + } + z -= 400; + } + printf_P(PSTR("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) +{ + printf_P(PSTR("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE); +// printf_P(PSTR("sizeof(pixels)=%d\n"), 32*32); +// printf_P(PSTR("sizeof(histo)=%d\n"), 2*16); +// printf_P(PSTR("sizeof(pattern)=%d\n"), 2*12); + printf_P(PSTR("sizeof(total)=%d\n"), 32*32+2*16+2*12); + bool ret = false; + int16_t x = (int16_t)count_position[X_AXIS]; + int16_t y = (int16_t)count_position[Y_AXIS]; + int16_t z = (int16_t)count_position[Z_AXIS]; + + 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)); +// printf_P(PSTR(" 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) + { + printf_P(PSTR(" pattern found at %d %d\n"), c, r); + c += 6; + r += 6; + x += ((int16_t)c - 16) << 6; + y += ((int16_t)r - 16) << 6; + printf_P(PSTR(" 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) +{ + printf_P(PSTR("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 = (int16_t)count_position[X_AXIS]; + int16_t y = (int16_t)count_position[Y_AXIS]; + int16_t z = (int16_t)count_position[Z_AXIS]; + 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)); + printf_P(PSTR("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 = (int16_t)count_position[Z_AXIS]; + xyzcal_lineXYZ_to(x, y, z, 200, 0); + if (xyzcal_scan_and_process()) + { + ret = true; + } +/* + x = pgm_read_word_far((uint16_t*)(xyzcal_point_xcoords_ + point)); + y = pgm_read_word_far((uint16_t*)(xyzcal_point_ycoords_ + point)); + printf_P(PSTR("point=%d x=%d y=%d z=%d\n"), point, x, y, z); + xyzcal_lineXYZ_to(x, y, z, 200, 0); + xyzcal_stepZ_dn_while_off(-2400, 500); + xyzcal_stepZ_up_while_on(800, 500); + ret = true;*/ + } + xyzcal_meassure_leave(); + return ret; +} + +bool xyzcal_improve_bed_induction_sensor_point(void) +{ + printf_P(PSTR("xyzcal_improve_bed_induction_sensor_point x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); + st_synchronize(); + xyzcal_meassure_enter(); + xyzcal_meassure_leave(); + return true; +} + +bool xyzcal_improve_bed_induction_sensor_point2(bool lift_z_on_min_y) +{ + printf_P(PSTR("xyzcal_improve_bed_induction_sensor_point2 x=%ld y=%ld z=%ld lift_z_on_min_y=%d\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS], lift_z_on_min_y?1:0); + return true; +} + +bool xyzcal_improve_bed_induction_sensor_point3(void) +{ + printf_P(PSTR("xyzcal_improve_bed_induction_sensor_point3 x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); + return true; +} + diff --git a/Firmware/xyzcal.h b/Firmware/xyzcal.h new file mode 100644 index 000000000..59af4b249 --- /dev/null +++ b/Firmware/xyzcal.h @@ -0,0 +1,47 @@ + +#include + + +extern void xyzcal_meassure_enter(void); + +extern void xyzcal_meassure_leave(void); + +extern int8_t xyzcal_read_pinda(void); + +extern uint16_t xyzcal_stepZ_up_while_on(int16_t max_z, uint16_t delay_us); + +extern uint16_t xyzcal_stepZ_dn_while_off(int16_t min_z, uint16_t delay_us); + +extern bool xyzcal_lineXYZ_by(int16_t dx, int16_t dy, int16_t dz, uint16_t delay_us, int8_t check_pinda); + +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); + +extern bool xyzcal_improve_bed_induction_sensor_point(void); + +extern bool xyzcal_improve_bed_induction_sensor_point2(bool lift_z_on_min_y); + +extern bool xyzcal_improve_bed_induction_sensor_point3(void); From 1850a57b728d6c2810919b273fe7b5bc61bb7dd2 Mon Sep 17 00:00:00 2001 From: Robert Pelnar Date: Tue, 27 Mar 2018 15:39:35 +0200 Subject: [PATCH 2/3] Revert "New XYZ calibration with image processing" This reverts commit 18b76d17dbf93f516129d5937456751b516b532e. --- Firmware/Configuration_prusa.h | 2 +- Firmware/Dcodes.cpp | 163 ------- Firmware/Dcodes.h | 3 - Firmware/Marlin_main.cpp | 27 +- Firmware/mesh_bed_calibration.cpp | 64 +-- Firmware/sm4.c | 180 ------- Firmware/sm4.h | 45 -- Firmware/temperature.h | 4 - Firmware/xyzcal.cpp | 782 ------------------------------ Firmware/xyzcal.h | 47 -- 10 files changed, 19 insertions(+), 1298 deletions(-) delete mode 100644 Firmware/sm4.c delete mode 100644 Firmware/sm4.h delete mode 100644 Firmware/xyzcal.cpp delete mode 100644 Firmware/xyzcal.h diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h index 26ce09286..47939e07a 100644 --- a/Firmware/Configuration_prusa.h +++ b/Firmware/Configuration_prusa.h @@ -172,7 +172,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o #define FANCHECK //#define WATCHDOG //#define SAFETYTIMER -#define NEW_XYZCAL + /*------------------------------------ LOAD/UNLOAD FILAMENT SETTINGS diff --git a/Firmware/Dcodes.cpp b/Firmware/Dcodes.cpp index cc9a698aa..7f7681739 100644 --- a/Firmware/Dcodes.cpp +++ b/Firmware/Dcodes.cpp @@ -470,169 +470,6 @@ void dcode_12() eeprom_update_byte((uint8_t*)EEPROM_POWER_COUNT, 0x00); } -#include "xyzcal.h" -void sync_command_P(const char* cmd, bool wait_movements) -{ - enquecommand_front_P(cmd); - process_commands(); - cmdqueue_pop_front(); - if (wait_movements) - st_synchronize(); -} - -#include "tmc2130.h" - -extern long count_position[NUM_AXIS]; - -void dcode_15() -{//PINDA scan - int p = -1; - if (code_seen('P')) - { - printf_P(PSTR("code seen P\n")); - p = code_value(); - switch (p) - { - case 0: enquecommand_front_P((PSTR("G1 X12 Y6 F6000"))); break; - case 1: enquecommand_front_P((PSTR("G1 X220 Y6 F6000"))); break; - case 2: enquecommand_front_P((PSTR("G1 X220 Y198 F6000"))); break; - case 3: enquecommand_front_P((PSTR("G1 X12 Y198 F6000"))); break; - } - return; - } - if (code_seen('H')) - {// meassure pinda hysterezis - printf_P(PSTR("code seen H\n")); - st_synchronize(); - if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS])) - { - sync_command_P(PSTR("G28W"), true); - } - sync_command_P(PSTR("G1 X12 Y6 Z2"), true); - xyzcal_meassure_enter(); - uint16_t pinda_hysterezis = xyzcal_meassure_pinda_hysterezis(-80, 5000, 500, 5); - xyzcal_meassure_leave(); - printf_P(PSTR("pinda_hysterezis = %d\n"), pinda_hysterezis); - return; - } - if (code_seen('U')) - {// Z up while pinda on - printf_P(PSTR("code seen U\n")); - uint16_t steps = xyzcal_stepZ_up_while_on(5000, 500); - printf_P(PSTR(" steps = %d\n"), steps); - return; - } - if (code_seen('B')) - {// Z down while pinda off - printf_P(PSTR("code seen B\n")); - uint16_t steps = xyzcal_stepZ_dn_while_off(-80, 500); - printf_P(PSTR(" steps = %d\n"), steps); - return; - } - if (code_seen('X')) - {// print counter positions - printf_P(PSTR("code seen X\n")); - printf_P(PSTR("X=%d Y=%d Z=%d\n"), (int16_t)count_position[X_AXIS], (int16_t)count_position[Y_AXIS], (int16_t)count_position[Z_AXIS]); - return; - } - if (code_seen('Y')) - {// print counter positions - printf_P(PSTR("code seen X\n")); - printf_P(PSTR("X=%ld Y=%ld Z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); - return; - } - if (code_seen('O')) - {// print counter positions - printf_P(PSTR("code seen O\n")); - xyzcal_meassure_enter(); - int16_t x = (int16_t)count_position[X_AXIS]; - int16_t y = (int16_t)count_position[Y_AXIS]; - int16_t z = (int16_t)count_position[Z_AXIS]; - xyzcal_find_point_center(x, y, z, z-10, z+10, 500, 10); - xyzcal_meassure_leave(); -// printf_P(PSTR("X=%d Y=%d Z=%d\n"), (int16_t)count_position[X_AXIS], (int16_t)count_position[Y_AXIS], (int16_t)count_position[Z_AXIS]); - return; - } - if (code_seen('S')) - {// print counter positions - printf_P(PSTR("code seen S\n")); - int16_t cx = (int16_t)count_position[X_AXIS]; - int16_t cy = (int16_t)count_position[Y_AXIS]; - int16_t z = (int16_t)count_position[Z_AXIS]; - xyzcal_meassure_enter(); - xyzcal_scan_pixels_32x32(cx, cy, z-96, z+383, 200, 0); - xyzcal_meassure_leave(); -// printf_P(PSTR("X=%d Y=%d Z=%d\n"), (int16_t)count_position[X_AXIS], (int16_t)count_position[Y_AXIS], (int16_t)count_position[Z_AXIS]); - return; - } - - if (code_seen('L')) - {// print counter positions - printf_P(PSTR("code seen L\n")); - int16_t x = (int16_t)count_position[X_AXIS]; - int16_t y = (int16_t)count_position[Y_AXIS]; - int16_t z = (int16_t)count_position[Z_AXIS]; - if (code_seen('x')) x = code_value(); - if (code_seen('y')) y = code_value(); - if (code_seen('z')) z = code_value(); - xyzcal_meassure_enter(); - xyzcal_lineXYZ_to(x, y, z, 320, 0); - xyzcal_meassure_leave(); -// printf_P(PSTR("X=%d Y=%d Z=%d\n"), (int16_t)count_position[X_AXIS], (int16_t)count_position[Y_AXIS], (int16_t)count_position[Z_AXIS]); - return; - } - printf_P(PSTR("no code seen \n")); - return; -/* - xyzcal_meassure_enter(); - - tmc2130_set_dir(X_AXIS, 0); - - int z = 0; - - int8_t _pinda = xyzcal_read_pinda(); - for (int x = 0; x < 10000; x++) - { - int8_t pinda = xyzcal_read_pinda(); - if (_pinda != pinda) - { - if (pinda) - printf_P(PSTR("!1 x=%d z=%d\n"), x, z+23); - else - printf_P(PSTR("!0 x=%d z=%d\n"), x, z-24); - _pinda = pinda; - } - tmc2130_set_dir(Z_AXIS, !pinda); - if (!pinda) - { - if (z > 0) - { - tmc2130_do_step(Z_AXIS); - z--; - } - } - else - { - tmc2130_do_step(Z_AXIS); - z++; - } - tmc2130_do_step(X_AXIS); - delayMicroseconds(400); - } - xyzcal_meassure_leave(); -return; -*/ -} - -void dcode_16() -{ - xyzcal_find_bed_induction_sensor_point_xy(); - return; -// xyzcal_meassure_enter(); -// xyzcal_searchZ(); -// xyzcal_meassure_leave(); -} - #ifdef TMC2130 #include "planner.h" diff --git a/Firmware/Dcodes.h b/Firmware/Dcodes.h index 081619045..69a55b13b 100644 --- a/Firmware/Dcodes.h +++ b/Firmware/Dcodes.h @@ -17,9 +17,6 @@ extern void dcode_9(); //D9 - Read/Write ADC (Write=enable simulated, Read=disab extern void dcode_10(); //D10 - XYZ calibration = OK extern void dcode_12(); //D12 - Reset failstat counters -extern void dcode_15(); //D15 -extern void dcode_16(); //D16 - #ifdef TMC2130 extern void dcode_2130(); //D2130 - TMC2130 #endif //TMC2130 diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index d5078875e..102482401 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -1306,7 +1306,7 @@ void setup() KEEPALIVE_STATE(NOT_BUSY); #ifdef WATCHDOG - wdt_enable(WDTO_4S); + wdt_enable(WDTO_4S); #endif //WATCHDOG } @@ -2228,7 +2228,7 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level) 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) { #ifdef HEATBED_V2 @@ -2251,7 +2251,6 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level) // if (result >= 0) babystep_apply(); #endif //HEATBED_V2 } -#endif //NEW_XYZCAL lcd_bed_calibration_show_result(result, point_too_far_mask); if (result >= 0) @@ -3229,16 +3228,6 @@ void process_commands() #ifdef PINDA_THERMISTOR if (true) { - 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; - 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); if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS])) { // We don't know where we are! HOME! // Push the commands to the front of the message queue in the reverse order! @@ -3247,6 +3236,10 @@ 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_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"); @@ -6413,12 +6406,6 @@ Sigma_Exit: case 12: //D12 - Reset failstat counters dcode_12(); break; - case 15: //D15 - - dcode_15(); break; - - case 16: //D16 - - dcode_16(); break; - #ifdef TMC2130 case 2130: // D9125 - TMC2130 dcode_2130(); break; @@ -6915,7 +6902,7 @@ void kill(const char *full_screen_message, unsigned char id) while(1) { #ifdef WATCHDOG - wdt_reset(); + wdt_reset(); #endif //WATCHDOG /* Intentionally left empty */ diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp index 973956543..8db94cd42 100644 --- a/Firmware/mesh_bed_calibration.cpp +++ b/Firmware/mesh_bed_calibration.cpp @@ -19,11 +19,8 @@ float world2machine_shift[2]; #define WEIGHT_FIRST_ROW_Y_HIGH (0.3f) #define WEIGHT_FIRST_ROW_Y_LOW (0.0f) -//#define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER) // -22 + 23 = 1 -//#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER) // -0.6 + 5 = 4.4 - #define BED_ZERO_REF_X (- 22.f + X_PROBE_OFFSET_FROM_EXTRUDER) // -22 + 23 = 1 -#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER + 4) // -0.6 + 5 = 4.4 +#define BED_ZERO_REF_Y (- 0.6f + Y_PROBE_OFFSET_FROM_EXTRUDER) // -0.6 + 5 = 4.4 // Scaling of the real machine axes against the programmed dimensions in the firmware. // The correction is tiny, here around 0.5mm on 250mm length. @@ -59,10 +56,10 @@ const float bed_skew_angle_extreme = (0.25f * M_PI / 180.f); // Positions of the bed reference points in the machine coordinates, referenced to the P.I.N.D.A sensor. // The points are the following: center front, center right, center rear, center left. const float bed_ref_points_4[] PROGMEM = { - 13.f - BED_ZERO_REF_X, 10.4f - BED_ZERO_REF_Y, - 221.f - BED_ZERO_REF_X, 10.4f - BED_ZERO_REF_Y, - 221.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y, - 13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y + 13.f - BED_ZERO_REF_X, 10.4f - 4.f - BED_ZERO_REF_Y, + 221.f - BED_ZERO_REF_X, 10.4f - 4.f - BED_ZERO_REF_Y, + 221.f - BED_ZERO_REF_X, 202.4f - 4.f - BED_ZERO_REF_Y, + 13.f - BED_ZERO_REF_X, 202.4f - 4.f - BED_ZERO_REF_Y }; const float bed_ref_points[] PROGMEM = { @@ -908,10 +905,6 @@ 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. @@ -925,13 +918,9 @@ extern bool xyzcal_find_bed_induction_sensor_point_xy(); #define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f) #endif //HEATBED_V2 - #ifdef HEATBED_V2 -/*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 @@ -1174,9 +1163,8 @@ extern bool xyzcal_find_bed_induction_sensor_point_xy(); enable_z_endstop(false); invert_z_endstop(false); return found; -#endif //NEW_XYZCAL -} +} #else //HEATBED_V2 inline bool find_bed_induction_sensor_point_xy(int verbosity_level) { @@ -1376,17 +1364,11 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level) #endif //HEATBED_V2 -#ifdef NEW_XYZCAL -extern bool xyzcal_improve_bed_induction_sensor_point(void); -#endif //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. -/*inline */bool improve_bed_induction_sensor_point() +inline bool improve_bed_induction_sensor_point() { -#ifdef NEW_XYZCAL - return xyzcal_improve_bed_induction_sensor_point(); -#else //NEW_XYZCAL static const float search_radius = 8.f; bool endstops_enabled = enable_endstops(false); @@ -1470,7 +1452,6 @@ extern bool xyzcal_improve_bed_induction_sensor_point(void); enable_endstops(endstops_enabled); enable_z_endstop(endstop_z_enabled); return found; -#endif //NEW_XYZCAL } static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z) @@ -1486,19 +1467,12 @@ static inline void debug_output_point(const char *type, const float &x, const fl SERIAL_ECHOLNPGM(""); } -#ifdef NEW_XYZCAL -extern bool xyzcal_improve_bed_induction_sensor_point2(bool lift_z_on_min_y); -#endif //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. #define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f) -/*inline */bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level) +inline bool improve_bed_induction_sensor_point2(bool lift_z_on_min_y, int8_t verbosity_level) { -#ifdef NEW_XYZCAL - return xyzcal_improve_bed_induction_sensor_point(); -#else //NEW_XYZCAL - float center_old_x = current_position[X_AXIS]; float center_old_y = current_position[Y_AXIS]; float a, b; @@ -1651,23 +1625,16 @@ canceled: enable_z_endstop(false); go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f); return false; -#endif //NEW_XYZCAL } -#ifdef NEW_XYZCAL -extern bool xyzcal_improve_bed_induction_sensor_point3(void); -#endif //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. // If this function failed, the Y coordinate will never be outside the working space. #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_RADIUS (8.f) #define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f) -/*inline */bool improve_bed_induction_sensor_point3(int verbosity_level) +inline bool improve_bed_induction_sensor_point3(int verbosity_level) { -#ifdef NEW_XYZCAL - return xyzcal_improve_bed_induction_sensor_point3(); -#else //NEW_XYZCAL float center_old_x = current_position[X_AXIS]; float center_old_y = current_position[Y_AXIS]; float a, b; @@ -1979,10 +1946,8 @@ canceled: current_position[Y_AXIS] = Y_MIN_POS; 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. @@ -2027,7 +1992,6 @@ 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 @@ -2415,11 +2379,7 @@ BedSkewOffsetDetectionResultType improve_bed_offset_and_skew(int8_t method, int8 current_position[Z_AXIS] -= 0.025f; // Improve the point position by searching its center in a current plane. int8_t n_errors = 3; -#ifdef NEW_XYZCAL - for (int8_t iter = 0; iter < 1; ) { -#else //NEW_XYZCAL - for (int8_t iter = 0; iter < 8; ) { -#endif //NEW_XYZCAL + for (int8_t iter = 0; iter < 8; ) { #ifdef SUPPORT_VERBOSITY if (verbosity_level > 20) { SERIAL_ECHOPGM("Improving bed point "); @@ -2772,7 +2732,6 @@ 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. @@ -2834,7 +2793,6 @@ 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/sm4.c b/Firmware/sm4.c deleted file mode 100644 index 1168abf28..000000000 --- a/Firmware/sm4.c +++ /dev/null @@ -1,180 +0,0 @@ -//sm4.c - simple 4-axis stepper control - -#include "sm4.h" -#include - -#include "boards.h" -#define bool int8_t -#define false 0 -#define true 1 -#include "Configuration_prusa.h" - -/**/ - -//direction signal pinout - 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 pinout - 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_update_pos_cb sm4_update_pos = 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)) -} - -int isqrt(int n) -{ - int a = 1; - int b = n; - while (abs(a - b) > 1) - { - b = n / a; - a = (a + b) / 2; - } - return a; -} - -uint8_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t delay_us, sm4_stop_cb stop_cb) -{ - 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; - uint8_t stop = 0; - while ((nd--) && stop_cb && !(stop = (*stop_cb)())) - { - 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); - delayMicroseconds(delay_us); - } - if (sm4_update_pos) (*sm4_update_pos)(x, y, z, 0); - return stop; -} - diff --git a/Firmware/sm4.h b/Firmware/sm4.h deleted file mode 100644 index 46416b4ab..000000000 --- a/Firmware/sm4.h +++ /dev/null @@ -1,45 +0,0 @@ -//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)(); - -// callback prototype for updating position counters -typedef void (*sm4_update_pos_cb)(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de); - -// callback pointer -extern sm4_update_pos_cb sm4_update_pos; - -// returns direction for single axis -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 -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 msk); - -// step axes by bitmask -extern void sm4_do_step(uint8_t axes_mask); - -// xyz linear-interpolated relative move -uint8_t sm4_line_xyz_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t delay_us, sm4_stop_cb stop); - - -#if defined(__cplusplus) -} -#endif //defined(__cplusplus) -#endif //_SM4_H diff --git a/Firmware/temperature.h b/Firmware/temperature.h index ec5670516..91791c4cf 100644 --- a/Firmware/temperature.h +++ b/Firmware/temperature.h @@ -27,10 +27,6 @@ #include "stepper.h" #endif -#define ENABLE_TEMPERATURE_INTERRUPT() TIMSK0 |= (1< -#include "stepper.h" -#include "temperature.h" -#include "sm4.h" -//#include "tmc2130.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 _PI 3.14159265F - -extern long count_position[NUM_AXIS]; - -void xyzcal_meassure_enter(void) -{ - printf_P(PSTR("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 -} - -void xyzcal_meassure_leave(void) -{ - printf_P(PSTR("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 -} - -int8_t xyzcal_read_pinda(void) -{ - return ((READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)?1:0); -} - -uint16_t xyzcal_stepZ_up_while_on(int16_t max_z, uint16_t delay_us) -{ -// printf_P(PSTR("xyzcal_stepZ_up_while_on %d\n"), max_z); - if (!xyzcal_read_pinda()) return 0; - uint16_t steps = 0; - sm4_set_dir(Z_AXIS, 0); - while (xyzcal_read_pinda() && (count_position[Z_AXIS] < (long)max_z)) - { - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us); - count_position[Z_AXIS]++; - steps++; - } - return steps; -} - -uint16_t xyzcal_stepZ_dn_while_off(int16_t min_z, uint16_t delay_us) -{ -// printf_P(PSTR("xyzcal_stepZ_dn_while_off %d\n"), min_z); - if (xyzcal_read_pinda()) return 0; - uint16_t steps = 0; - sm4_set_dir(Z_AXIS, 1); - while (!xyzcal_read_pinda() && (count_position[Z_AXIS] > (long)min_z)) - { - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us); - count_position[Z_AXIS]--; - steps++; - } - return steps; -} - -void xyzcal_stepZ_by(int16_t z_delta, uint16_t delay_us) -{ - sm4_set_dir(Z_AXIS, (z_delta < 0)?1:0); - while (z_delta) - { - sm4_do_step(Z_AXIS_MASK); - delayMicroseconds(delay_us); - if (z_delta > 0) - { - count_position[Z_AXIS]++; - z_delta--; - } - else - { - count_position[Z_AXIS]--; - z_delta++; - } - } -} - -void xyzcal_stepZ_to(int16_t z_target, uint16_t delay_us) -{ -// printf_P(PSTR("xyzcal_stepZ_to %d\n"), z_target); - xyzcal_stepZ_by(z_target - count_position[Z_AXIS], delay_us); -} - -bool xyzcal_lineXYZ_ui(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t delay_us, int8_t check_pinda) -{ -// printf_P(PSTR("xyzcal_lineXYZ_ui %u %u %u %u %d\n"), dx, dy, dz, delay_us, check_pinda); - uint16_t d = (uint16_t)(sqrt((float)(((uint32_t)dx)*dx + ((uint32_t)dy*dy) + ((uint32_t)dz*dz))) + 0.5); -// printf_P(PSTR(" d = %u\n"), d); - uint16_t cx = d; - uint16_t cy = d; - uint16_t cz = d; - uint16_t nx = dx; - uint16_t ny = dy; - uint16_t nz = dz; - uint8_t msk = 0; -// int8_t pinda = 0; - int8_t pinda = xyzcal_read_pinda(); -// printf_P(PSTR(" pinda = %d\n"), pinda); - while (nx || ny || nz) - { - msk = 0; - if (cx <= dx) - { - msk |= 1; - nx--; - cx += d; - } - if (cy <= dy) - { - msk |= 2; - ny--; - cy += d; - } - if (cz <= dz) - { - msk |= 4; - nz--; - cz += d; - } - cx -= dx; - cy -= dy; - cz -= dz; - sm4_do_step(msk); - delayMicroseconds(delay_us); - if (check_pinda) - { - pinda = xyzcal_read_pinda(); - if ((check_pinda > 0) && pinda) break; - if ((check_pinda < 0) && !pinda) break; - } -// printf_P(PSTR("%d %d %d %d\n"), nx, ny, nz, delay_us); - } - if (sm4_get_dir(X_AXIS)) count_position[X_AXIS] -= (dx - nx); - else count_position[X_AXIS] += (dx - nx); - if (sm4_get_dir(Y_AXIS)) count_position[Y_AXIS] -= (dy - ny); - else count_position[Y_AXIS] += (dy - ny); - if (sm4_get_dir(Z_AXIS)) count_position[Z_AXIS] -= (dz - nz); - else count_position[Z_AXIS] += (dz - nz); - if ((check_pinda > 0) && pinda) - { -// int8_t pinda = xyzcal_read_pinda(); -// printf_P(PSTR(" pinda = %d\n"), pinda); -// printf_P(PSTR("PINDA 0>1\n")); - return true; - } - if ((check_pinda < 0) && !pinda) - { -// int8_t pinda = xyzcal_read_pinda(); -// printf_P(PSTR(" pinda = %d\n"), pinda); -// printf_P(PSTR("PINDA 1>0\n")); - return true; - } - return false; -} - -uint8_t check_pinda_cb_0() -{ - return xyzcal_read_pinda()?0:1; -} - -uint8_t check_pinda_cb_1() -{ - return xyzcal_read_pinda()?1:0; -} - -uint8_t xyzcal_dm = 0; - -void xyzcal_update_pos_cb(uint16_t dx, uint16_t dy, uint16_t dz, uint16_t de) -{ - if (dx) count_position[0] += (xyzcal_dm&1)?-dx:dx; - if (dy) count_position[1] += (xyzcal_dm&2)?-dy:dy; - if (dz) count_position[2] += (xyzcal_dm&4)?-dz:dz; -} - -bool xyzcal_lineXYZ(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t 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) | ((z<0)?2:0) | ((y<0)?4:0); - sm4_set_dir_bits(xyzcal_dm); - sm4_line_xyz_ui(x, y, z, delay_us, check_pinda?((check_pinda<0)?check_pinda_cb_0:check_pinda_cb_1):0)?true:false; -} - -bool xyzcal_lineXYZ_by(int16_t dx, int16_t dy, int16_t dz, uint16_t delay_us, int8_t check_pinda) -{ - if (dx > 0) sm4_set_dir(X_AXIS, 0); - else if (dx < 0) sm4_set_dir(X_AXIS, 1); - if (dy > 0) sm4_set_dir(Y_AXIS, 0); - else if (dy < 0) sm4_set_dir(Y_AXIS, 1); - if (dz > 0) sm4_set_dir(Z_AXIS, 0); - else if (dz < 0) sm4_set_dir(Z_AXIS, 1); - return xyzcal_lineXYZ_ui(abs(dx), abs(dy), abs(dz), delay_us, check_pinda); -} - -bool xyzcal_lineXYZ_to(int16_t x, int16_t y, int16_t z, uint16_t delay_us, int8_t check_pinda) -{ -// printf_P(PSTR(" xyzcal_lineXYZ_to %d %d %d %u\n"), x, y, z, delay_us); - return xyzcal_lineXYZ_by(x - (int16_t)count_position[0], y - (int16_t)count_position[1], z - (int16_t)count_position[2], 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; - 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; - printf_P(PSTR("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; - printf_P(PSTR("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; -} - -int8_t xyzcal_meassure_pinda_hysterezis(int16_t min_z, int16_t max_z, uint16_t delay_us, uint8_t samples) -{ -// printf_P(PSTR("xyzcal_meassure_pinda_hysterezis\n")); - int8_t ret = -1; // PINDA signal error - int16_t z = (int16_t)count_position[Z_AXIS]; - int16_t sum_up = 0; - int16_t sum_dn = 0; - int16_t up; - int16_t dn; - uint8_t sample; - dn = xyzcal_stepZ_dn_while_off(min_z, delay_us); -// printf_P(PSTR("dn=%d\n"), sample, up, dn); - up = xyzcal_stepZ_up_while_on(max_z, delay_us); -// printf_P(PSTR("up=%d\n"), sample, up, dn); - if (!xyzcal_read_pinda()) - { - for (sample = 0; sample < samples; sample++) - { - dn = xyzcal_stepZ_dn_while_off(min_z, 2*delay_us); - if (!xyzcal_read_pinda()) break; - up = xyzcal_stepZ_up_while_on(max_z, 2*delay_us); - if (xyzcal_read_pinda()) break; -// printf_P(PSTR("%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_stepZ_to(z, delay_us); //return to original Z position - return ret; -} - -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) -{ - printf_P(PSTR("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 = xyzcal_read_pinda(); - xyzcal_lineXYZ_to((r&1)?(cx+1024):(cx-1024), cy - 1024 + r*64, z, 2*delay_us, 0); - xyzcal_stepZ_dn_while_off(min_z, 2*delay_us); - xyzcal_stepZ_up_while_on(max_z, 2*delay_us); - 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 = xyzcal_read_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) -// printf_P(PSTR("!1 x=%d z=%d\n"), c*64+i, z+23); -// else -// printf_P(PSTR("!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; -// printf_P(PSTR("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++) - printf_P(PSTR("%02x"), pixels[((uint16_t)r<<5) + c]); - printf_P(PSTR("\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++) - printf_P(PSTR(" %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; - } - } - printf_P(PSTR("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; - printf_P(PSTR(" 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++) - printf_P(PSTR("%02x"), pixels[((uint16_t)r<<5) + c]); - printf_P(PSTR("\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; - } -// printf("%2d %2d %d\n", x, y, match8x8(0, pixels, w, h, x, y)); - } - printf("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; -} - -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 = xyzcal_read_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 = xyzcal_read_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) - printf_P(PSTR("!1 x=%d y=%d\n"), x, y); - else - printf_P(PSTR("!0 x=%d y=%d\n"), x, y); - }*/ - _pinda = pinda; -// printf_P(PSTR("x=%d y=%d rx=%d ry=%d\n"), x, y, rx, ry); - } - printf_P(PSTR("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; - printf_P(PSTR("x0=%d y0=%d r=%d\n"), x0, y0, r); - } - } - xyzcal_lineXYZ_to(x0, y0, z, 200, 0); -} - -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; -} - -//const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200}; -//const int16_t PROGMEM xyzcal_point_ycoords[4] = {600, 600, 19800, 19800}; -const int16_t PROGMEM xyzcal_point_xcoords[4] = {1200, 22000, 22000, 1200}; -const int16_t PROGMEM xyzcal_point_ycoords[4] = {700, 700, 19800, 19800}; - -const int16_t PROGMEM xyzcal_point_xcoords_[4] = {1131, 21939, 21964, 1122}; -const int16_t PROGMEM xyzcal_point_ycoords_[4] = {709, 674, 19883, 19922}; - -const uint16_t PROGMEM xyzcal_point_pattern[12] = {0x000, 0x0f0, 0x1f8, 0x3fc, 0x7fe, 0x7fe, 0x7fe, 0x7fe, 0x3fc, 0x1f8, 0x0f0, 0x000}; -/* -int16_t xyzcal_point2xcoord(uint8_t point) -{ - return xyzcal_point_xcoords[point & 3]; -} - -int16_t xyzcal_point2ycoord(uint8_t point) -{ - return xyzcal_point_ycoords[point & 3]; -} -*/ -bool xyzcal_searchZ(void) -{ - printf_P(PSTR("xyzcal_searchZ x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); - int16_t x0 = (int16_t)count_position[X_AXIS]; - int16_t y0 = (int16_t)count_position[Y_AXIS]; - int16_t z0 = (int16_t)count_position[Z_AXIS]; -// 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 = (int16_t)count_position[X_AXIS]; - int16_t y_on = (int16_t)count_position[Y_AXIS]; - int16_t z_on = (int16_t)count_position[Z_AXIS]; - printf_P(PSTR(" ON-SIGNAL at x=%d y=%d z=%d ad=%d\n"), x_on, y_on, z_on, ad); - return true; - } - z -= 400; - } - printf_P(PSTR("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) -{ - printf_P(PSTR("sizeof(block_buffer)=%d\n"), sizeof(block_t)*BLOCK_BUFFER_SIZE); -// printf_P(PSTR("sizeof(pixels)=%d\n"), 32*32); -// printf_P(PSTR("sizeof(histo)=%d\n"), 2*16); -// printf_P(PSTR("sizeof(pattern)=%d\n"), 2*12); - printf_P(PSTR("sizeof(total)=%d\n"), 32*32+2*16+2*12); - bool ret = false; - int16_t x = (int16_t)count_position[X_AXIS]; - int16_t y = (int16_t)count_position[Y_AXIS]; - int16_t z = (int16_t)count_position[Z_AXIS]; - - 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)); -// printf_P(PSTR(" 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) - { - printf_P(PSTR(" pattern found at %d %d\n"), c, r); - c += 6; - r += 6; - x += ((int16_t)c - 16) << 6; - y += ((int16_t)r - 16) << 6; - printf_P(PSTR(" 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) -{ - printf_P(PSTR("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 = (int16_t)count_position[X_AXIS]; - int16_t y = (int16_t)count_position[Y_AXIS]; - int16_t z = (int16_t)count_position[Z_AXIS]; - 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)); - printf_P(PSTR("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 = (int16_t)count_position[Z_AXIS]; - xyzcal_lineXYZ_to(x, y, z, 200, 0); - if (xyzcal_scan_and_process()) - { - ret = true; - } -/* - x = pgm_read_word_far((uint16_t*)(xyzcal_point_xcoords_ + point)); - y = pgm_read_word_far((uint16_t*)(xyzcal_point_ycoords_ + point)); - printf_P(PSTR("point=%d x=%d y=%d z=%d\n"), point, x, y, z); - xyzcal_lineXYZ_to(x, y, z, 200, 0); - xyzcal_stepZ_dn_while_off(-2400, 500); - xyzcal_stepZ_up_while_on(800, 500); - ret = true;*/ - } - xyzcal_meassure_leave(); - return ret; -} - -bool xyzcal_improve_bed_induction_sensor_point(void) -{ - printf_P(PSTR("xyzcal_improve_bed_induction_sensor_point x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); - st_synchronize(); - xyzcal_meassure_enter(); - xyzcal_meassure_leave(); - return true; -} - -bool xyzcal_improve_bed_induction_sensor_point2(bool lift_z_on_min_y) -{ - printf_P(PSTR("xyzcal_improve_bed_induction_sensor_point2 x=%ld y=%ld z=%ld lift_z_on_min_y=%d\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS], lift_z_on_min_y?1:0); - return true; -} - -bool xyzcal_improve_bed_induction_sensor_point3(void) -{ - printf_P(PSTR("xyzcal_improve_bed_induction_sensor_point3 x=%ld y=%ld z=%ld\n"), count_position[X_AXIS], count_position[Y_AXIS], count_position[Z_AXIS]); - return true; -} - diff --git a/Firmware/xyzcal.h b/Firmware/xyzcal.h deleted file mode 100644 index 59af4b249..000000000 --- a/Firmware/xyzcal.h +++ /dev/null @@ -1,47 +0,0 @@ - -#include - - -extern void xyzcal_meassure_enter(void); - -extern void xyzcal_meassure_leave(void); - -extern int8_t xyzcal_read_pinda(void); - -extern uint16_t xyzcal_stepZ_up_while_on(int16_t max_z, uint16_t delay_us); - -extern uint16_t xyzcal_stepZ_dn_while_off(int16_t min_z, uint16_t delay_us); - -extern bool xyzcal_lineXYZ_by(int16_t dx, int16_t dy, int16_t dz, uint16_t delay_us, int8_t check_pinda); - -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); - -extern bool xyzcal_improve_bed_induction_sensor_point(void); - -extern bool xyzcal_improve_bed_induction_sensor_point2(bool lift_z_on_min_y); - -extern bool xyzcal_improve_bed_induction_sensor_point3(void); From a51ab82a6151be69b519a02b0d97ae5eb66291f0 Mon Sep 17 00:00:00 2001 From: Robert Pelnar Date: Tue, 27 Mar 2018 19:27:07 +0200 Subject: [PATCH 3/3] New XYZ calibration with image processing --- Firmware/Configuration_prusa.h | 4 + Firmware/Marlin_main.cpp | 2 + Firmware/mesh_bed_calibration.cpp | 29 +- Firmware/mesh_bed_calibration.h | 2 + Firmware/sm4.c | 194 ++++++++ Firmware/sm4.h | 56 +++ Firmware/xyzcal.cpp | 773 ++++++++++++++++++++++++++++++ Firmware/xyzcal.h | 41 ++ 8 files changed, 1100 insertions(+), 1 deletion(-) create mode 100644 Firmware/sm4.c create mode 100644 Firmware/sm4.h create mode 100644 Firmware/xyzcal.cpp create mode 100644 Firmware/xyzcal.h diff --git a/Firmware/Configuration_prusa.h b/Firmware/Configuration_prusa.h index 47939e07a..ad98fa0bd 100644 --- a/Firmware/Configuration_prusa.h +++ b/Firmware/Configuration_prusa.h @@ -170,6 +170,10 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o #define PAT9125 //!< Filament sensor #define FANCHECK + +// New XYZ calibration +#define NEW_XYZCAL + //#define WATCHDOG //#define SAFETYTIMER diff --git a/Firmware/Marlin_main.cpp b/Firmware/Marlin_main.cpp index 70b77f680..25fcab963 100644 --- a/Firmware/Marlin_main.cpp +++ b/Firmware/Marlin_main.cpp @@ -2252,6 +2252,8 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level) // if (result >= 0) babystep_apply(); #endif //HEATBED_V2 } + lcd_update_enable(true); + lcd_update(2); lcd_bed_calibration_show_result(result, point_too_far_mask); if (result >= 0) diff --git a/Firmware/mesh_bed_calibration.cpp b/Firmware/mesh_bed_calibration.cpp index 322acbef1..f6cb70d86 100644 --- a/Firmware/mesh_bed_calibration.cpp +++ b/Firmware/mesh_bed_calibration.cpp @@ -905,6 +905,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. @@ -921,6 +924,9 @@ error: #ifdef HEATBED_V2 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 @@ -1163,11 +1169,14 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level) enable_z_endstop(false); invert_z_endstop(false); return found; - +#endif //NEW_XYZCAL } #else //HEATBED_V2 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 @@ -1360,10 +1369,13 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level) enable_z_endstop(false); return found; +#endif //NEW_XYZCAL } #endif //HEATBED_V2 +#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. @@ -1453,7 +1465,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 "); @@ -1466,7 +1480,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. @@ -1626,7 +1642,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. @@ -1947,7 +1965,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. @@ -1992,6 +2012,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 @@ -2118,6 +2139,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; +#ifndef NEW_XYZCAL #ifndef HEATBED_V2 if (k == 0 || k == 1) { @@ -2140,6 +2162,7 @@ BedSkewOffsetDetectionResultType find_bed_offset_and_skew(int8_t verbosity_level return BED_SKEW_OFFSET_DETECTION_POINT_NOT_FOUND; } #endif //HEATBED_V2 +#endif #ifdef SUPPORT_VERBOSITY if (verbosity_level >= 10) delay_keep_alive(3000); @@ -2280,6 +2303,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. @@ -2576,6 +2600,7 @@ canceled: enable_z_endstop(endstop_z_enabled); return result; } +#endif //NEW_XYZCAL bool sample_z() { bool sampled = true; @@ -2732,6 +2757,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. @@ -2793,6 +2819,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 5f5a98aa2..5dd2202e1 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 000000000..165426cf8 --- /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 == 310)) + 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 == 310)) + 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 == 310)) + 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 == 310)) + 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 == 310)) + 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 == 310)) +} + +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 000000000..fc64f7a66 --- /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/xyzcal.cpp b/Firmware/xyzcal.cpp new file mode 100644 index 000000000..b59918f70 --- /dev/null +++ b/Firmware/xyzcal.cpp @@ -0,0 +1,773 @@ +//xyzcal.cpp - xyz calibration with image processing + +#include "Configuration_prusa.h" +#ifdef NEW_XYZCAL + +#include "xyzcal.h" +#include +#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 + 2) << 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 = 32; + 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; +} + +#define MAX_DIAMETR 600 + +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); + + z0 = _Z; + +// 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; + + 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); + + xyzcal_lineXYZ_to(x0, y0, z0, delay_us, 0); + + return 1; +} + +#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 == 310)) +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 == 310)) + +//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 - 72, 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()) + { + 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; + } + } + } + } + xyzcal_meassure_leave(); + return ret; +} + + +#endif //NEW_XYZCAL diff --git a/Firmware/xyzcal.h b/Firmware/xyzcal.h new file mode 100644 index 000000000..5590396e4 --- /dev/null +++ b/Firmware/xyzcal.h @@ -0,0 +1,41 @@ +//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_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); + +extern bool xyzcal_find_bed_induction_sensor_point_xy(void); + + +#endif //_XYZCAL_H