New XYZ calibration with image processing
This commit is contained in:
Robert Pelnar
parent
e3967e444b
commit
18b76d17db
|
|
@ -172,7 +172,7 @@ const bool Z_MIN_ENDSTOP_INVERTING = false; // set to true to invert the logic o
|
||||||
#define FANCHECK
|
#define FANCHECK
|
||||||
//#define WATCHDOG
|
//#define WATCHDOG
|
||||||
//#define SAFETYTIMER
|
//#define SAFETYTIMER
|
||||||
|
#define NEW_XYZCAL
|
||||||
|
|
||||||
/*------------------------------------
|
/*------------------------------------
|
||||||
LOAD/UNLOAD FILAMENT SETTINGS
|
LOAD/UNLOAD FILAMENT SETTINGS
|
||||||
|
|
|
||||||
|
|
@ -470,6 +470,169 @@ void dcode_12()
|
||||||
eeprom_update_byte((uint8_t*)EEPROM_POWER_COUNT, 0x00);
|
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
|
#ifdef TMC2130
|
||||||
#include "planner.h"
|
#include "planner.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_10(); //D10 - XYZ calibration = OK
|
||||||
extern void dcode_12(); //D12 - Reset failstat counters
|
extern void dcode_12(); //D12 - Reset failstat counters
|
||||||
|
|
||||||
|
extern void dcode_15(); //D15
|
||||||
|
extern void dcode_16(); //D16
|
||||||
|
|
||||||
#ifdef TMC2130
|
#ifdef TMC2130
|
||||||
extern void dcode_2130(); //D2130 - TMC2130
|
extern void dcode_2130(); //D2130 - TMC2130
|
||||||
#endif //TMC2130
|
#endif //TMC2130
|
||||||
|
|
|
||||||
|
|
@ -1306,7 +1306,7 @@ void setup()
|
||||||
|
|
||||||
KEEPALIVE_STATE(NOT_BUSY);
|
KEEPALIVE_STATE(NOT_BUSY);
|
||||||
#ifdef WATCHDOG
|
#ifdef WATCHDOG
|
||||||
wdt_enable(WDTO_4S);
|
wdt_enable(WDTO_4S);
|
||||||
#endif //WATCHDOG
|
#endif //WATCHDOG
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -2228,7 +2228,7 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level)
|
||||||
current_position[Z_AXIS] = MESH_HOME_Z_SEARCH;
|
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);
|
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();
|
st_synchronize();
|
||||||
|
#ifndef NEW_XYZCAL
|
||||||
if (result >= 0)
|
if (result >= 0)
|
||||||
{
|
{
|
||||||
#ifdef HEATBED_V2
|
#ifdef HEATBED_V2
|
||||||
|
|
@ -2251,6 +2251,7 @@ bool gcode_M45(bool onlyZ, int8_t verbosity_level)
|
||||||
// if (result >= 0) babystep_apply();
|
// if (result >= 0) babystep_apply();
|
||||||
#endif //HEATBED_V2
|
#endif //HEATBED_V2
|
||||||
}
|
}
|
||||||
|
#endif //NEW_XYZCAL
|
||||||
|
|
||||||
lcd_bed_calibration_show_result(result, point_too_far_mask);
|
lcd_bed_calibration_show_result(result, point_too_far_mask);
|
||||||
if (result >= 0)
|
if (result >= 0)
|
||||||
|
|
@ -3228,6 +3229,16 @@ void process_commands()
|
||||||
#ifdef PINDA_THERMISTOR
|
#ifdef PINDA_THERMISTOR
|
||||||
if (true)
|
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])) {
|
if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS] && axis_known_position[Z_AXIS])) {
|
||||||
// We don't know where we are! HOME!
|
// We don't know where we are! HOME!
|
||||||
// Push the commands to the front of the message queue in the reverse order!
|
// 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")));
|
enquecommand_front_P((PSTR("G28 W0")));
|
||||||
break;
|
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
|
KEEPALIVE_STATE(NOT_BUSY); //no need to print busy messages as we print current temperatures periodicaly
|
||||||
SERIAL_ECHOLNPGM("PINDA probe calibration start");
|
SERIAL_ECHOLNPGM("PINDA probe calibration start");
|
||||||
|
|
||||||
|
|
@ -6406,6 +6413,12 @@ Sigma_Exit:
|
||||||
case 12: //D12 - Reset failstat counters
|
case 12: //D12 - Reset failstat counters
|
||||||
dcode_12(); break;
|
dcode_12(); break;
|
||||||
|
|
||||||
|
case 15: //D15 -
|
||||||
|
dcode_15(); break;
|
||||||
|
|
||||||
|
case 16: //D16 -
|
||||||
|
dcode_16(); break;
|
||||||
|
|
||||||
#ifdef TMC2130
|
#ifdef TMC2130
|
||||||
case 2130: // D9125 - TMC2130
|
case 2130: // D9125 - TMC2130
|
||||||
dcode_2130(); break;
|
dcode_2130(); break;
|
||||||
|
|
@ -6902,7 +6915,7 @@ void kill(const char *full_screen_message, unsigned char id)
|
||||||
while(1)
|
while(1)
|
||||||
{
|
{
|
||||||
#ifdef WATCHDOG
|
#ifdef WATCHDOG
|
||||||
wdt_reset();
|
wdt_reset();
|
||||||
#endif //WATCHDOG
|
#endif //WATCHDOG
|
||||||
/* Intentionally left empty */
|
/* Intentionally left empty */
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -19,8 +19,11 @@ float world2machine_shift[2];
|
||||||
#define WEIGHT_FIRST_ROW_Y_HIGH (0.3f)
|
#define WEIGHT_FIRST_ROW_Y_HIGH (0.3f)
|
||||||
#define WEIGHT_FIRST_ROW_Y_LOW (0.0f)
|
#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_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.
|
// Scaling of the real machine axes against the programmed dimensions in the firmware.
|
||||||
// The correction is tiny, here around 0.5mm on 250mm length.
|
// 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.
|
// 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.
|
// The points are the following: center front, center right, center rear, center left.
|
||||||
const float bed_ref_points_4[] PROGMEM = {
|
const float bed_ref_points_4[] PROGMEM = {
|
||||||
13.f - BED_ZERO_REF_X, 10.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 - 4.f - BED_ZERO_REF_Y,
|
221.f - BED_ZERO_REF_X, 10.4f - BED_ZERO_REF_Y,
|
||||||
221.f - BED_ZERO_REF_X, 202.4f - 4.f - BED_ZERO_REF_Y,
|
221.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y,
|
||||||
13.f - BED_ZERO_REF_X, 202.4f - 4.f - BED_ZERO_REF_Y
|
13.f - BED_ZERO_REF_X, 202.4f - BED_ZERO_REF_Y
|
||||||
};
|
};
|
||||||
|
|
||||||
const float bed_ref_points[] PROGMEM = {
|
const float bed_ref_points[] PROGMEM = {
|
||||||
|
|
@ -905,6 +908,10 @@ error:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifdef NEW_XYZCAL
|
||||||
|
extern bool xyzcal_find_bed_induction_sensor_point_xy();
|
||||||
|
#endif //NEW_XYZCAL
|
||||||
|
|
||||||
// Search around the current_position[X,Y],
|
// Search around the current_position[X,Y],
|
||||||
// look for the induction sensor response.
|
// 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.
|
// 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)
|
#define FIND_BED_INDUCTION_SENSOR_POINT_Z_STEP (0.2f)
|
||||||
#endif //HEATBED_V2
|
#endif //HEATBED_V2
|
||||||
|
|
||||||
|
|
||||||
#ifdef 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
|
#ifdef SUPPORT_VERBOSITY
|
||||||
if (verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
|
if (verbosity_level >= 10) MYSERIAL.println("find bed induction sensor point xy");
|
||||||
#endif // SUPPORT_VERBOSITY
|
#endif // SUPPORT_VERBOSITY
|
||||||
|
|
@ -1163,8 +1174,9 @@ inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
|
||||||
enable_z_endstop(false);
|
enable_z_endstop(false);
|
||||||
invert_z_endstop(false);
|
invert_z_endstop(false);
|
||||||
return found;
|
return found;
|
||||||
|
#endif //NEW_XYZCAL
|
||||||
}
|
}
|
||||||
|
|
||||||
#else //HEATBED_V2
|
#else //HEATBED_V2
|
||||||
inline bool find_bed_induction_sensor_point_xy(int verbosity_level)
|
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
|
#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].
|
// Search around the current_position[X,Y,Z].
|
||||||
// It is expected, that the induction sensor is switched on at the current position.
|
// 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.
|
// 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;
|
static const float search_radius = 8.f;
|
||||||
|
|
||||||
bool endstops_enabled = enable_endstops(false);
|
bool endstops_enabled = enable_endstops(false);
|
||||||
|
|
@ -1452,6 +1470,7 @@ inline bool improve_bed_induction_sensor_point()
|
||||||
enable_endstops(endstops_enabled);
|
enable_endstops(endstops_enabled);
|
||||||
enable_z_endstop(endstop_z_enabled);
|
enable_z_endstop(endstop_z_enabled);
|
||||||
return found;
|
return found;
|
||||||
|
#endif //NEW_XYZCAL
|
||||||
}
|
}
|
||||||
|
|
||||||
static inline void debug_output_point(const char *type, const float &x, const float &y, const float &z)
|
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("");
|
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].
|
// Search around the current_position[X,Y,Z].
|
||||||
// It is expected, that the induction sensor is switched on at the current position.
|
// 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.
|
// Look around this center point by painting a star around the point.
|
||||||
#define IMPROVE_BED_INDUCTION_SENSOR_SEARCH_RADIUS (8.f)
|
#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_x = current_position[X_AXIS];
|
||||||
float center_old_y = current_position[Y_AXIS];
|
float center_old_y = current_position[Y_AXIS];
|
||||||
float a, b;
|
float a, b;
|
||||||
|
|
@ -1625,16 +1651,23 @@ canceled:
|
||||||
enable_z_endstop(false);
|
enable_z_endstop(false);
|
||||||
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
||||||
return false;
|
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 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.
|
// 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.
|
// 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.
|
// 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_RADIUS (8.f)
|
||||||
#define IMPROVE_BED_INDUCTION_SENSOR_POINT3_SEARCH_STEP_FINE_Y (0.1f)
|
#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_x = current_position[X_AXIS];
|
||||||
float center_old_y = current_position[Y_AXIS];
|
float center_old_y = current_position[Y_AXIS];
|
||||||
float a, b;
|
float a, b;
|
||||||
|
|
@ -1946,8 +1979,10 @@ canceled:
|
||||||
current_position[Y_AXIS] = Y_MIN_POS;
|
current_position[Y_AXIS] = Y_MIN_POS;
|
||||||
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
||||||
return false;
|
return false;
|
||||||
|
#endif //NEW_XYZCAL
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifndef NEW_XYZCAL
|
||||||
// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
|
// Scan the mesh bed induction points one by one by a left-right zig-zag movement,
|
||||||
// write the trigger coordinates to the serial line.
|
// write the trigger coordinates to the serial line.
|
||||||
// Useful for visualizing the behavior of the bed induction detector.
|
// 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;
|
current_position[Y_AXIS] = center_old_y;
|
||||||
go_xy(current_position[X_AXIS], current_position[Y_AXIS], homing_feedrate[X_AXIS] / 60.f);
|
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
|
#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;
|
current_position[Z_AXIS] -= 0.025f;
|
||||||
// Improve the point position by searching its center in a current plane.
|
// Improve the point position by searching its center in a current plane.
|
||||||
int8_t n_errors = 3;
|
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
|
#ifdef SUPPORT_VERBOSITY
|
||||||
if (verbosity_level > 20) {
|
if (verbosity_level > 20) {
|
||||||
SERIAL_ECHOPGM("Improving bed point ");
|
SERIAL_ECHOPGM("Improving bed point ");
|
||||||
|
|
@ -2732,6 +2772,7 @@ bool sample_mesh_and_store_reference()
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifndef NEW_XYZCAL
|
||||||
bool scan_bed_induction_points(int8_t verbosity_level)
|
bool scan_bed_induction_points(int8_t verbosity_level)
|
||||||
{
|
{
|
||||||
// Don't let the manage_inactivity() function remove power from the motors.
|
// 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);
|
enable_z_endstop(endstop_z_enabled);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
#endif //NEW_XYZCAL
|
||||||
|
|
||||||
// Shift a Z axis by a given delta.
|
// Shift a Z axis by a given delta.
|
||||||
// To replace loading of the babystep correction.
|
// To replace loading of the babystep correction.
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,180 @@
|
||||||
|
//sm4.c - simple 4-axis stepper control
|
||||||
|
|
||||||
|
#include "sm4.h"
|
||||||
|
#include <avr/io.h>
|
||||||
|
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,45 @@
|
||||||
|
//sm4.h - simple 4-axis stepper control
|
||||||
|
#ifndef _SM4_H
|
||||||
|
#define _SM4_H
|
||||||
|
|
||||||
|
#include <inttypes.h>
|
||||||
|
#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
|
||||||
|
|
@ -27,6 +27,10 @@
|
||||||
#include "stepper.h"
|
#include "stepper.h"
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#define ENABLE_TEMPERATURE_INTERRUPT() TIMSK0 |= (1<<OCIE0B)
|
||||||
|
#define DISABLE_TEMPERATURE_INTERRUPT() TIMSK0 &= ~(1<<OCIE0B)
|
||||||
|
|
||||||
|
|
||||||
// public functions
|
// public functions
|
||||||
void tp_init(); //initialize the heating
|
void tp_init(); //initialize the heating
|
||||||
void manage_heater(); //it is critical that this is called periodically.
|
void manage_heater(); //it is critical that this is called periodically.
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,782 @@
|
||||||
|
|
||||||
|
#include "xyzcal.h"
|
||||||
|
#include <avr/wdt.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -0,0 +1,47 @@
|
||||||
|
|
||||||
|
#include <inttypes.h>
|
||||||
|
|
||||||
|
|
||||||
|
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);
|
||||||
Loading…
Reference in New Issue