|
|
|
@ -25,37 +25,49 @@ |
|
|
|
#if ENABLED(Z_STEPPER_AUTO_ALIGN) |
|
|
|
|
|
|
|
#include "../gcode.h" |
|
|
|
#include "../../module/delta.h" |
|
|
|
#include "../../module/motion.h" |
|
|
|
#include "../../module/planner.h" |
|
|
|
#include "../../module/stepper.h" |
|
|
|
#include "../../module/endstops.h" |
|
|
|
#include "../../module/motion.h" |
|
|
|
#include "../../module/probe.h" |
|
|
|
|
|
|
|
#if HOTENDS > 1 |
|
|
|
#include "../../module/tool_change.h" |
|
|
|
#endif |
|
|
|
|
|
|
|
#if HAS_BED_PROBE |
|
|
|
#include "../../module/probe.h" |
|
|
|
#endif |
|
|
|
|
|
|
|
#if HAS_LEVELING |
|
|
|
#include "../../feature/bedlevel/bedlevel.h" |
|
|
|
#endif |
|
|
|
|
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
#include "../../libs/least_squares_fit.h" |
|
|
|
#endif |
|
|
|
|
|
|
|
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) |
|
|
|
#include "../../core/debug_out.h" |
|
|
|
|
|
|
|
// Sanity-check
|
|
|
|
// Sanity-check the count of Z_STEPPER_ALIGN_XY points
|
|
|
|
constexpr xy_pos_t sanity_arr_z_align[] = Z_STEPPER_ALIGN_XY; |
|
|
|
static_assert(COUNT(sanity_arr_z_align) == Z_STEPPER_COUNT, |
|
|
|
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS) |
|
|
|
"Z_STEPPER_ALIGN_XY requires three {X,Y} entries (Z, Z2, and Z3)." |
|
|
|
#else |
|
|
|
"Z_STEPPER_ALIGN_XY requires two {X,Y} entries (Z and Z2)." |
|
|
|
#endif |
|
|
|
); |
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
static_assert(COUNT(sanity_arr_z_align) >= Z_STEPPER_COUNT, |
|
|
|
"Z_STEPPER_ALIGN_XY requires at least three {X,Y} entries (Z, Z2, Z3, ...)." |
|
|
|
); |
|
|
|
#else |
|
|
|
static_assert(COUNT(sanity_arr_z_align) == Z_STEPPER_COUNT, |
|
|
|
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS) |
|
|
|
"Z_STEPPER_ALIGN_XY requires three {X,Y} entries (Z, Z2, and Z3)." |
|
|
|
#else |
|
|
|
"Z_STEPPER_ALIGN_XY requires two {X,Y} entries (Z and Z2)." |
|
|
|
#endif |
|
|
|
); |
|
|
|
#endif |
|
|
|
|
|
|
|
static xy_pos_t z_auto_align_pos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_XY; |
|
|
|
|
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
static xy_pos_t z_stepper_pos[] = Z_STEPPER_ALIGN_STEPPER_XY; |
|
|
|
#endif |
|
|
|
|
|
|
|
xy_pos_t z_auto_align_pos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_XY; |
|
|
|
#define G34_PROBE_COUNT COUNT(z_auto_align_pos) |
|
|
|
|
|
|
|
inline void set_all_z_lock(const bool lock) { |
|
|
|
stepper.set_z_lock(lock); |
|
|
|
@ -68,7 +80,9 @@ inline void set_all_z_lock(const bool lock) { |
|
|
|
/**
|
|
|
|
* G34: Z-Stepper automatic alignment |
|
|
|
* |
|
|
|
* Parameters: I<iterations> T<accuracy> A<amplification> |
|
|
|
* I<iterations> |
|
|
|
* T<accuracy> |
|
|
|
* A<amplification> |
|
|
|
*/ |
|
|
|
void GcodeSuite::G34() { |
|
|
|
if (DEBUGGING(LEVELING)) { |
|
|
|
@ -90,11 +104,18 @@ void GcodeSuite::G34() { |
|
|
|
break; |
|
|
|
} |
|
|
|
|
|
|
|
const float z_auto_align_amplification = parser.floatval('A', Z_STEPPER_ALIGN_AMP); |
|
|
|
if (!WITHIN(ABS(z_auto_align_amplification), 0.5f, 2.0f)) { |
|
|
|
SERIAL_ECHOLNPGM("?(A)mplification out of bounds (0.5-2.0)."); |
|
|
|
break; |
|
|
|
} |
|
|
|
const float z_auto_align_amplification = |
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
Z_STEPPER_ALIGN_AMP; |
|
|
|
#else |
|
|
|
parser.floatval('A', Z_STEPPER_ALIGN_AMP); |
|
|
|
if (!WITHIN(ABS(z_auto_align_amplification), 0.5f, 2.0f)) { |
|
|
|
SERIAL_ECHOLNPGM("?(A)mplification out of bounds (0.5-2.0)."); |
|
|
|
break; |
|
|
|
} |
|
|
|
#endif |
|
|
|
|
|
|
|
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; |
|
|
|
|
|
|
|
// Wait for planner moves to finish!
|
|
|
|
planner.synchronize(); |
|
|
|
@ -130,11 +151,13 @@ void GcodeSuite::G34() { |
|
|
|
#define Z_BASIC_CLEARANCE Z_CLEARANCE_BETWEEN_PROBES |
|
|
|
#endif |
|
|
|
|
|
|
|
// Compute a worst-case clearance height to probe from. After the first
|
|
|
|
// iteration this will be re-calculated based on the actual bed position
|
|
|
|
float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * ( |
|
|
|
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS) |
|
|
|
SQRT(_MAX(HYPOT2(z_auto_align_pos[0].x - z_auto_align_pos[0].y, z_auto_align_pos[1].x - z_auto_align_pos[1].y), |
|
|
|
HYPOT2(z_auto_align_pos[1].x - z_auto_align_pos[1].y, z_auto_align_pos[2].x - z_auto_align_pos[2].y), |
|
|
|
HYPOT2(z_auto_align_pos[2].x - z_auto_align_pos[2].y, z_auto_align_pos[0].x - z_auto_align_pos[0].y))) |
|
|
|
HYPOT2(z_auto_align_pos[1].x - z_auto_align_pos[1].y, z_auto_align_pos[2].x - z_auto_align_pos[2].y), |
|
|
|
HYPOT2(z_auto_align_pos[2].x - z_auto_align_pos[2].y, z_auto_align_pos[0].x - z_auto_align_pos[0].y))) |
|
|
|
#else |
|
|
|
HYPOT(z_auto_align_pos[0].x - z_auto_align_pos[0].y, z_auto_align_pos[1].x - z_auto_align_pos[1].y) |
|
|
|
#endif |
|
|
|
@ -147,12 +170,10 @@ void GcodeSuite::G34() { |
|
|
|
current_position.z -= z_probe * 0.5f; |
|
|
|
|
|
|
|
float last_z_align_move[Z_STEPPER_COUNT] = ARRAY_N(Z_STEPPER_COUNT, 10000.0f, 10000.0f, 10000.0f), |
|
|
|
z_measured[Z_STEPPER_COUNT] = { 0 }, |
|
|
|
z_measured[G34_PROBE_COUNT] = { 0 }, |
|
|
|
z_maxdiff = 0.0f, |
|
|
|
amplification = z_auto_align_amplification; |
|
|
|
|
|
|
|
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE; |
|
|
|
|
|
|
|
uint8_t iteration; |
|
|
|
bool err_break = false; |
|
|
|
for (iteration = 0; iteration < z_auto_align_iterations; ++iteration) { |
|
|
|
@ -161,17 +182,19 @@ void GcodeSuite::G34() { |
|
|
|
SERIAL_ECHOLNPAIR("\nITERATION: ", int(iteration + 1)); |
|
|
|
|
|
|
|
// Initialize minimum value
|
|
|
|
float z_measured_min = 100000.0f; |
|
|
|
float z_measured_min = 100000.0f, |
|
|
|
z_measured_max = -100000.0f; |
|
|
|
|
|
|
|
// Probe all positions (one per Z-Stepper)
|
|
|
|
for (uint8_t izstepper = 0; izstepper < Z_STEPPER_COUNT; ++izstepper) { |
|
|
|
for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) { |
|
|
|
// iteration odd/even --> downward / upward stepper sequence
|
|
|
|
const uint8_t zstepper = (iteration & 1) ? Z_STEPPER_COUNT - 1 - izstepper : izstepper; |
|
|
|
const uint8_t iprobe = (iteration & 1) ? G34_PROBE_COUNT - 1 - i : i; |
|
|
|
|
|
|
|
// Safe clearance even on an incline
|
|
|
|
if (iteration == 0 || izstepper > 0) do_blocking_move_to_z(z_probe); |
|
|
|
if (iteration == 0 || i > 0) do_blocking_move_to_z(z_probe); |
|
|
|
|
|
|
|
// Probe a Z height for each stepper.
|
|
|
|
const float z_probed_height = probe_at_point(z_auto_align_pos[zstepper], raise_after, 0, true); |
|
|
|
const float z_probed_height = probe_at_point(z_auto_align_pos[i], raise_after, 0, true); |
|
|
|
if (isnan(z_probed_height)) { |
|
|
|
SERIAL_ECHOLNPGM("Probing failed."); |
|
|
|
err_break = true; |
|
|
|
@ -180,35 +203,58 @@ void GcodeSuite::G34() { |
|
|
|
|
|
|
|
// Add height to each value, to provide a more useful target height for
|
|
|
|
// the next iteration of probing. This allows adjustments to be made away from the bed.
|
|
|
|
z_measured[zstepper] = z_probed_height + Z_CLEARANCE_BETWEEN_PROBES; |
|
|
|
z_measured[iprobe] = z_probed_height + Z_CLEARANCE_BETWEEN_PROBES; |
|
|
|
|
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(zstepper + 1), " measured position is ", z_measured[zstepper]); |
|
|
|
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("> Z", int(iprobe + 1), " measured position is ", z_measured[iprobe]); |
|
|
|
|
|
|
|
// Remember the minimum measurement to calculate the correction later on
|
|
|
|
z_measured_min = _MIN(z_measured_min, z_measured[zstepper]); |
|
|
|
} // for (zstepper)
|
|
|
|
z_measured_min = _MIN(z_measured_min, z_measured[iprobe]); |
|
|
|
z_measured_max = _MAX(z_measured_max, z_measured[iprobe]); |
|
|
|
} // for (i)
|
|
|
|
|
|
|
|
if (err_break) break; |
|
|
|
|
|
|
|
// Adapt the next probe clearance height based on the new measurements.
|
|
|
|
// Safe_height = lowest distance to bed (= highest measurement) plus highest measured misalignment.
|
|
|
|
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS) |
|
|
|
z_maxdiff = _MAX(ABS(z_measured[0] - z_measured[1]), ABS(z_measured[1] - z_measured[2]), ABS(z_measured[2] - z_measured[0])); |
|
|
|
z_probe = Z_BASIC_CLEARANCE + _MAX(z_measured[0], z_measured[1], z_measured[2]) + z_maxdiff; |
|
|
|
#else |
|
|
|
z_maxdiff = ABS(z_measured[0] - z_measured[1]); |
|
|
|
z_probe = Z_BASIC_CLEARANCE + _MAX(z_measured[0], z_measured[1]) + z_maxdiff; |
|
|
|
z_maxdiff = z_measured_max - z_measured_min; |
|
|
|
z_probe = Z_BASIC_CLEARANCE + z_measured_max + z_maxdiff; |
|
|
|
|
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
// Replace the initial values in z_measured with calculated heights at
|
|
|
|
// each stepper position. This allows the adjustment algorithm to be
|
|
|
|
// shared between both possible probing mechanisms.
|
|
|
|
|
|
|
|
// This must be done after the next z_probe height is calculated, so that
|
|
|
|
// the height is calculated from actual print area positions, and not
|
|
|
|
// extrapolated motor movements.
|
|
|
|
|
|
|
|
// Compute the least-squares fit for all probed points.
|
|
|
|
// Calculate the Z position of each stepper and store it in z_measured.
|
|
|
|
// This allows the actual adjustment logic to be shared by both algorithms.
|
|
|
|
linear_fit_data lfd; |
|
|
|
incremental_LSF_reset(&lfd); |
|
|
|
for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) { |
|
|
|
SERIAL_ECHOLNPAIR("PROBEPT_", int(i + 1), ": ", z_measured[i]); |
|
|
|
incremental_LSF(&lfd, z_auto_align_pos[i], z_measured[i]); |
|
|
|
} |
|
|
|
finish_incremental_LSF(&lfd); |
|
|
|
|
|
|
|
z_measured_min = 100000.0f; |
|
|
|
for (uint8_t i = 0; i < Z_STEPPER_COUNT; ++i) { |
|
|
|
z_measured[i] = -(lfd.A * z_stepper_pos[i].x + lfd.B * z_stepper_pos[i].y); |
|
|
|
z_measured_min = _MIN(z_measured_min, z_measured[i]); |
|
|
|
} |
|
|
|
|
|
|
|
SERIAL_ECHOLNPAIR("CALCULATED STEPPER POSITIONS: Z1=", z_measured[0], " Z2=", z_measured[1], " Z3=", z_measured[2]); |
|
|
|
#endif |
|
|
|
|
|
|
|
SERIAL_ECHOPAIR("\n" |
|
|
|
SERIAL_ECHOLNPAIR("\n" |
|
|
|
"DIFFERENCE Z1-Z2=", ABS(z_measured[0] - z_measured[1]) |
|
|
|
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS) |
|
|
|
, " Z2-Z3=", ABS(z_measured[1] - z_measured[2]) |
|
|
|
, " Z3-Z1=", ABS(z_measured[2] - z_measured[0]) |
|
|
|
#endif |
|
|
|
); |
|
|
|
SERIAL_EOL(); |
|
|
|
SERIAL_EOL(); |
|
|
|
|
|
|
|
// The following correction actions are to be enabled for select Z-steppers only
|
|
|
|
stepper.set_separate_multi_axis(true); |
|
|
|
@ -220,8 +266,10 @@ void GcodeSuite::G34() { |
|
|
|
const float z_align_move = z_measured[zstepper] - z_measured_min, |
|
|
|
z_align_abs = ABS(z_align_move); |
|
|
|
|
|
|
|
// Optimize one iterations correction based on the first measurements
|
|
|
|
if (z_align_abs > 0.0f) amplification = iteration == 1 ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification; |
|
|
|
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
// Optimize one iteration's correction based on the first measurements
|
|
|
|
if (z_align_abs > 0.0f) amplification = iteration == 1 ? _MIN(last_z_align_move[zstepper] / z_align_abs, 2.0f) : z_auto_align_amplification; |
|
|
|
#endif |
|
|
|
|
|
|
|
// Check for less accuracy compared to last move
|
|
|
|
if (last_z_align_move[zstepper] < z_align_abs - 1.0) { |
|
|
|
@ -266,7 +314,6 @@ void GcodeSuite::G34() { |
|
|
|
|
|
|
|
SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " iterations of ", int(z_auto_align_iterations)); |
|
|
|
SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff); |
|
|
|
SERIAL_EOL(); |
|
|
|
|
|
|
|
// Restore the active tool after homing
|
|
|
|
#if HOTENDS > 1 |
|
|
|
@ -299,31 +346,82 @@ void GcodeSuite::G34() { |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* M422: Z-Stepper automatic alignment parameter selection |
|
|
|
* M422: Set a Z-Stepper automatic alignment XY point. |
|
|
|
* Use repeatedly to set multiple points. |
|
|
|
* |
|
|
|
* S<index> : Index of the probe point to set |
|
|
|
* |
|
|
|
* With Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS: |
|
|
|
* W<index> : Index of the Z stepper position to set |
|
|
|
* The W and S parameters may not be combined. |
|
|
|
* |
|
|
|
* S and W require an X and/or Y parameter |
|
|
|
* X<pos> : X position to set (Unchanged if omitted) |
|
|
|
* Y<pos> : Y position to set (Unchanged if omitted) |
|
|
|
*/ |
|
|
|
void GcodeSuite::M422() { |
|
|
|
const int8_t zstepper = parser.intval('S') - 1; |
|
|
|
if (!WITHIN(zstepper, 0, Z_STEPPER_COUNT - 1)) { |
|
|
|
SERIAL_ECHOLNPGM("?(S) Z-Stepper index invalid."); |
|
|
|
if (!parser.seen_any()) { |
|
|
|
for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) |
|
|
|
SERIAL_ECHOLNPAIR("M422 S", i + 1, " X", z_auto_align_pos[i].x, " Y", z_auto_align_pos[i].y); |
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
for (uint8_t i = 0; i < Z_STEPPER_COUNT; ++i) |
|
|
|
SERIAL_ECHOLNPAIR("M422 W", i + 1, " X", z_stepper_pos[i].x, " Y", z_stepper_pos[i].y); |
|
|
|
#endif |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
const xy_pos_t pos = { |
|
|
|
parser.floatval('X', z_auto_align_pos[zstepper].x), |
|
|
|
parser.floatval('Y', z_auto_align_pos[zstepper].y) |
|
|
|
}; |
|
|
|
const bool is_probe_point = parser.seen('S'); |
|
|
|
|
|
|
|
if (!WITHIN(pos.x, X_MIN_POS, X_MAX_POS)) { |
|
|
|
SERIAL_ECHOLNPGM("?(X) out of bounds."); |
|
|
|
return; |
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
if (is_probe_point && parser.seen('W')) { |
|
|
|
SERIAL_ECHOLNPGM("?(S) and (W) may not be combined."); |
|
|
|
return; |
|
|
|
} |
|
|
|
#endif |
|
|
|
|
|
|
|
xy_pos_t *pos_dest = ( |
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
!is_probe_point ? z_stepper_pos : |
|
|
|
#endif |
|
|
|
z_auto_align_pos |
|
|
|
); |
|
|
|
|
|
|
|
// Get the Probe Position Index or Z Stepper Index
|
|
|
|
int8_t position_index; |
|
|
|
if (is_probe_point) { |
|
|
|
position_index = parser.intval('S') - 1; |
|
|
|
if (!WITHIN(position_index, 0, int8_t(G34_PROBE_COUNT) - 1)) { |
|
|
|
SERIAL_ECHOLNPGM("?(S) Z-ProbePosition index invalid."); |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
else { |
|
|
|
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) |
|
|
|
position_index = parser.intval('W') - 1; |
|
|
|
if (!WITHIN(position_index, 0, Z_STEPPER_COUNT - 1)) { |
|
|
|
SERIAL_ECHOLNPGM("?(W) Z-Stepper index invalid."); |
|
|
|
return; |
|
|
|
} |
|
|
|
#endif |
|
|
|
} |
|
|
|
|
|
|
|
if (!WITHIN(pos.y, Y_MIN_POS, Y_MAX_POS)) { |
|
|
|
SERIAL_ECHOLNPGM("?(Y) out of bounds."); |
|
|
|
return; |
|
|
|
const xy_pos_t pos = { |
|
|
|
parser.floatval('X', pos_dest[position_index].x), |
|
|
|
parser.floatval('Y', pos_dest[position_index].y) |
|
|
|
}; |
|
|
|
|
|
|
|
if (is_probe_point) { |
|
|
|
if (!position_is_reachable_by_probe(pos.x, Y_CENTER)) { |
|
|
|
SERIAL_ECHOLNPGM("?(X) out of bounds."); |
|
|
|
return; |
|
|
|
} |
|
|
|
if (!position_is_reachable_by_probe(pos)) { |
|
|
|
SERIAL_ECHOLNPGM("?(Y) out of bounds."); |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
z_auto_align_pos[zstepper] = pos; |
|
|
|
pos_dest[position_index] = pos; |
|
|
|
} |
|
|
|
|
|
|
|
#endif // Z_STEPPER_AUTO_ALIGN
|
|
|
|
|
Jason Smith
5 years ago
Scott Lahteine