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);