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 WATCHDOG
|
||||
//#define SAFETYTIMER
|
||||
|
||||
#define NEW_XYZCAL
|
||||
|
||||
/*------------------------------------
|
||||
LOAD/UNLOAD FILAMENT SETTINGS
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
#endif
|
||||
|
||||
#define ENABLE_TEMPERATURE_INTERRUPT() TIMSK0 |= (1<<OCIE0B)
|
||||
#define DISABLE_TEMPERATURE_INTERRUPT() TIMSK0 &= ~(1<<OCIE0B)
|
||||
|
||||
|
||||
// public functions
|
||||
void tp_init(); //initialize the heating
|
||||
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