bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

G34 automatic point assignment (#16473)

Browse Source
This commit is contained in:
InsanityAutomation
2020-02-01 05:50:44 -05:00
committed by GitHub
parent 90b6324563
commit e58d1bf974
6 changed files with 298 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

132
Marlin/src/gcode/calibrate/G34_M422.cpp
View File

@ -24,6 +24,8 @@
#if ENABLED(Z_STEPPER_AUTO_ALIGN)
#include "../../feature/z_stepper_align.h"
#include "../gcode.h"
#include "../../module/planner.h"
#include "../../module/stepper.h"
@ -45,68 +47,6 @@
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
#include "../../core/debug_out.h"
//
// Sanity check G34 / M422 settings
//
constexpr xy_pos_t test_z_stepper_align_xy[] = Z_STEPPER_ALIGN_XY;
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
static_assert(COUNT(test_z_stepper_align_xy) >= NUM_Z_STEPPER_DRIVERS,
"Z_STEPPER_ALIGN_XY requires at least three {X,Y} entries (Z, Z2, Z3, ...)."
);
constexpr float test_z_stepper_align_stepper_xy[][XY] = Z_STEPPER_ALIGN_STEPPER_XY;
static_assert(
COUNT(test_z_stepper_align_stepper_xy) == NUM_Z_STEPPER_DRIVERS,
"Z_STEPPER_ALIGN_STEPPER_XY requires three {X,Y} entries (one per Z stepper)."
);
#else
static_assert(COUNT(test_z_stepper_align_xy) == NUM_Z_STEPPER_DRIVERS,
#if NUM_Z_STEPPER_DRIVERS == 4
"Z_STEPPER_ALIGN_XY requires four {X,Y} entries (Z, Z2, Z3, and Z4)."
#elif NUM_Z_STEPPER_DRIVERS == 3
"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
constexpr xyz_pos_t dpo = NOZZLE_TO_PROBE_OFFSET;
#define LTEST(N) (test_z_stepper_align_xy[N].x >= _MAX(X_MIN_BED + MIN_PROBE_EDGE_LEFT, X_MIN_POS + dpo.x) - 0.00001f)
#define RTEST(N) (test_z_stepper_align_xy[N].x <= _MIN(X_MAX_BED - MIN_PROBE_EDGE_RIGHT, X_MAX_POS + dpo.x) + 0.00001f)
#define FTEST(N) (test_z_stepper_align_xy[N].y >= _MAX(Y_MIN_BED + MIN_PROBE_EDGE_FRONT, Y_MIN_POS + dpo.y) - 0.00001f)
#define BTEST(N) (test_z_stepper_align_xy[N].y <= _MIN(Y_MAX_BED - MIN_PROBE_EDGE_BACK, Y_MAX_POS + dpo.y) + 0.00001f)
static_assert(LTEST(0) && RTEST(0), "The 1st Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset.");
static_assert(FTEST(0) && BTEST(0), "The 1st Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset.");
static_assert(LTEST(1) && RTEST(1), "The 2nd Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset.");
static_assert(FTEST(1) && BTEST(1), "The 2nd Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset.");
#if NUM_Z_STEPPER_DRIVERS >= 3
static_assert(LTEST(2) && RTEST(2), "The 3rd Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset.");
static_assert(FTEST(2) && BTEST(2), "The 3rd Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset.");
#if NUM_Z_STEPPER_DRIVERS >= 4
static_assert(LTEST(3) && RTEST(3), "The 4th Z_STEPPER_ALIGN_XY X is unreachable with the default probe X offset.");
static_assert(FTEST(3) && BTEST(3), "The 4th Z_STEPPER_ALIGN_XY Y is unreachable with the default probe Y offset.");
#endif
#endif
//
// G34 / M422 shared data
//
static xy_pos_t z_stepper_align_pos[] = Z_STEPPER_ALIGN_XY;
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
static xy_pos_t z_stepper_align_stepper_pos[] = Z_STEPPER_ALIGN_STEPPER_XY;
#endif
#define G34_PROBE_COUNT COUNT(z_stepper_align_pos)
inline void set_all_z_lock(const bool lock) {
stepper.set_z_lock(lock);
stepper.set_z2_lock(lock);
@ -201,11 +141,11 @@ void GcodeSuite::G34() {
// iteration this will be re-calculated based on the actual bed position
float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * (
#if NUM_Z_STEPPER_DRIVERS == 3
SQRT(_MAX(HYPOT2(z_stepper_align_pos[0].x - z_stepper_align_pos[0].y, z_stepper_align_pos[1].x - z_stepper_align_pos[1].y),
HYPOT2(z_stepper_align_pos[1].x - z_stepper_align_pos[1].y, z_stepper_align_pos[2].x - z_stepper_align_pos[2].y),
HYPOT2(z_stepper_align_pos[2].x - z_stepper_align_pos[2].y, z_stepper_align_pos[0].x - z_stepper_align_pos[0].y)))
SQRT(_MAX(HYPOT2(z_stepper_align.xy[0].x - z_stepper_align.xy[0].y, z_stepper_align.xy[1].x - z_stepper_align.xy[1].y),
HYPOT2(z_stepper_align.xy[1].x - z_stepper_align.xy[1].y, z_stepper_align.xy[2].x - z_stepper_align.xy[2].y),
HYPOT2(z_stepper_align.xy[2].x - z_stepper_align.xy[2].y, z_stepper_align.xy[0].x - z_stepper_align.xy[0].y)))
#else
HYPOT(z_stepper_align_pos[0].x - z_stepper_align_pos[0].y, z_stepper_align_pos[1].x - z_stepper_align_pos[1].y)
HYPOT(z_stepper_align.xy[0].x - z_stepper_align.xy[0].y, z_stepper_align.xy[1].x - z_stepper_align.xy[1].y)
#endif
);
@ -216,31 +156,39 @@ void GcodeSuite::G34() {
current_position.z -= z_probe * 0.5f;
float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f),
z_measured[G34_PROBE_COUNT] = { 0 },
z_measured[NUM_Z_STEPPER_DRIVERS] = { 0 },
z_maxdiff = 0.0f,
amplification = z_auto_align_amplification;
uint8_t iteration;
bool err_break = false;
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
bool adjustment_reverse = false;
#endif
for (iteration = 0; iteration < z_auto_align_iterations; ++iteration) {
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("> probing all positions.");
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 i = 0; i < G34_PROBE_COUNT; ++i) {
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) {
// iteration odd/even --> downward / upward stepper sequence
const uint8_t iprobe = (iteration & 1) ? G34_PROBE_COUNT - 1 - i : i;
const uint8_t iprobe = (iteration & 1) ? NUM_Z_STEPPER_DRIVERS - 1 - i : i;
// Safe clearance even on an incline
if (iteration == 0 || i > 0) do_blocking_move_to_z(z_probe);
if (DEBUGGING(LEVELING))
DEBUG_ECHOLNPAIR_P(PSTR("Probing X"), z_stepper_align.xy[iprobe].x, SP_Y_STR, z_stepper_align.xy[iprobe].y);
// Probe a Z height for each stepper.
const float z_probed_height = probe.probe_at_point(z_stepper_align_pos[iprobe], raise_after, 0, true);
const float z_probed_height = probe.probe_at_point(z_stepper_align.xy[iprobe], raise_after, 0, true);
if (isnan(z_probed_height)) {
SERIAL_ECHOLNPGM("Probing failed.");
err_break = true;
@ -279,15 +227,15 @@ void GcodeSuite::G34() {
// 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_stepper_align_pos[i], z_measured[i]);
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) {
SERIAL_ECHOLNPAIR("PROBEPT_", i + '1', ": ", z_measured[i]);
incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]);
}
finish_incremental_LSF(&lfd);
z_measured_min = 100000.0f;
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i) {
z_measured[i] = -(lfd.A * z_stepper_align_stepper_pos[i].x + lfd.B * z_stepper_align_stepper_pos[i].y);
z_measured[i] = -(lfd.A * z_stepper_align.stepper_xy[i].x + lfd.B * z_stepper_align.stepper_xy[i].y);
z_measured_min = _MIN(z_measured_min, z_measured[i]);
}
@ -309,8 +257,8 @@ void GcodeSuite::G34() {
// Correct the individual stepper offsets
for (uint8_t zstepper = 0; zstepper < NUM_Z_STEPPER_DRIVERS; ++zstepper) {
// Calculate current stepper move
const float z_align_move = z_measured[zstepper] - z_measured_min,
z_align_abs = ABS(z_align_move);
float z_align_move = z_measured[zstepper] - z_measured_min;
const float z_align_abs = ABS(z_align_move);
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
// Optimize one iteration's correction based on the first measurements
@ -318,10 +266,14 @@ void GcodeSuite::G34() {
#endif
// Check for less accuracy compared to last move
if (last_z_align_move[zstepper] < z_align_abs - 1.0) {
if (last_z_align_move[zstepper] < z_align_abs * 0.7f) {
SERIAL_ECHOLNPGM("Decreasing accuracy detected.");
err_break = true;
break;
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
adjustment_reverse = !adjustment_reverse;
#else
err_break = true;
break;
#endif
}
// Remember the alignment for the next iteration
@ -342,6 +294,13 @@ void GcodeSuite::G34() {
#endif
}
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
// Decreasing accuracy was detected so move was inverted.
// Will match reversed Z steppers on dual steppers. Triple will need more work to map.
if (adjustment_reverse)
z_align_move = -z_align_move;
#endif
// Do a move to correct part of the misalignment for the current stepper
do_blocking_move_to_z(amplification * z_align_move + current_position.z);
} // for (zstepper)
@ -406,12 +365,13 @@ void GcodeSuite::G34() {
* Y<pos> : Y position to set (Unchanged if omitted)
*/
void GcodeSuite::M422() {
if (!parser.seen_any()) {
for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i)
SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), i + 1, SP_X_STR, z_stepper_align_pos[i].x, SP_Y_STR, z_stepper_align_pos[i].y);
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i)
SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), i + '1', SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y);
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
for (uint8_t i = 0; i < NUM_Z_STEPPER_DRIVERS; ++i)
SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), i + 1, SP_X_STR, z_stepper_align_stepper_pos[i].x, SP_Y_STR, z_stepper_align_stepper_pos[i].y);
SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), i + '1', SP_X_STR, z_stepper_align.stepper_xy[i].x, SP_Y_STR, z_stepper_align.stepper_xy[i].y);
#endif
return;
}
@ -427,9 +387,9 @@ void GcodeSuite::M422() {
xy_pos_t *pos_dest = (
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
!is_probe_point ? z_stepper_align_stepper_pos :
!is_probe_point ? z_stepper_align.stepper_xy :
#endif
z_stepper_align_pos
z_stepper_align.xy
);
if (!is_probe_point
@ -451,7 +411,7 @@ void GcodeSuite::M422() {
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)) {
if (!WITHIN(position_index, 0, int8_t(NUM_Z_STEPPER_DRIVERS) - 1)) {
SERIAL_ECHOLNPGM("?(S) Z-ProbePosition index invalid.");
return;
}