Skip to content

Commit

Permalink
✨ Update G34 for 4x Z steppers (MarlinFirmware#22039)
Browse files Browse the repository at this point in the history
  • Loading branch information
@thinkyhead
thinkyhead committed Jun 15, 2021
1 parent aeb8097 commit c90fa53
Showing 1 changed file with 50 additions and 39 deletions.
89 changes: 50 additions & 39 deletions Marlin/src/gcode/calibrate/G34_M422.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,13 @@
#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE)
#include "../../core/debug_out.h"

#if NUM_Z_STEPPER_DRIVERS >= 3
#define TRIPLE_Z 1
#if NUM_Z_STEPPER_DRIVERS >= 4
#define QUAD_Z 1
#endif
#endif

/**
* G34: Z-Stepper automatic alignment
*
Expand Down Expand Up @@ -82,9 +89,9 @@ void GcodeSuite::G34() {
switch (parser.intval('Z')) {
case 1: stepper.set_z1_lock(state); break;
case 2: stepper.set_z2_lock(state); break;
#if NUM_Z_STEPPER_DRIVERS >= 3
#if TRIPLE_Z
case 3: stepper.set_z3_lock(state); break;
#if NUM_Z_STEPPER_DRIVERS >= 4
#if QUAD_Z
case 4: stepper.set_z4_lock(state); break;
#endif
#endif
Expand All @@ -99,13 +106,6 @@ void GcodeSuite::G34() {
#if ENABLED(Z_STEPPER_AUTO_ALIGN)
do { // break out on error

#if NUM_Z_STEPPER_DRIVERS == 4
SERIAL_ECHOLNPGM("Alignment for 4 steppers is Experimental!");
#elif NUM_Z_STEPPER_DRIVERS > 4
SERIAL_ECHOLNPGM("Alignment not supported for over 4 steppers");
break;
#endif

const int8_t z_auto_align_iterations = parser.intval('I', Z_STEPPER_ALIGN_ITERATIONS);
if (!WITHIN(z_auto_align_iterations, 1, 30)) {
SERIAL_ECHOLNPGM("?(I)teration out of bounds (1-30).");
Expand Down Expand Up @@ -157,16 +157,14 @@ void GcodeSuite::G34() {
const xy_pos_t diff = z_stepper_align.xy[i] - z_stepper_align.xy[j];
return HYPOT2(diff.x, diff.y);
};
float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT(
#if NUM_Z_STEPPER_DRIVERS == 3
_MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 0))
#elif NUM_Z_STEPPER_DRIVERS == 4
_MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 3),
magnitude2(3, 0), magnitude2(0, 2), magnitude2(1, 3))
#else
magnitude2(0, 1)
float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT(_MAX(0, magnitude2(0, 1)
#if TRIPLE_Z
, magnitude2(2, 1), magnitude2(2, 0)
#if QUAD_Z
, magnitude2(3, 2), magnitude2(3, 1), magnitude2(3, 0)
#endif
#endif
);
));

// Home before the alignment procedure
if (!all_axes_trusted()) home_all_axes();
Expand All @@ -178,7 +176,7 @@ void GcodeSuite::G34() {
// This hack is un-done at the end of G34 - either by re-homing, or by using the probed heights of the last iteration.

#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f, 10000.0f);
float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N_1(NUM_Z_STEPPER_DRIVERS, 10000.0f);
#else
float last_z_align_level_indicator = 10000.0f;
#endif
Expand Down Expand Up @@ -280,39 +278,52 @@ void GcodeSuite::G34() {
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]);
SERIAL_ECHOLNPAIR(
LIST_N(DOUBLE(NUM_Z_STEPPER_DRIVERS),
"Calculated Z1=", z_measured[0],
" Z2=", z_measured[1],
" Z3=", z_measured[2],
" Z4=", z_measured[3]
)
);
#endif

SERIAL_ECHOLNPAIR("\n"
"DIFFERENCE Z1-Z2=", ABS(z_measured[0] - z_measured[1])
#if NUM_Z_STEPPER_DRIVERS == 3
, " Z2-Z3=", ABS(z_measured[1] - z_measured[2])
"Z2-Z1=", ABS(z_measured[1] - z_measured[0])
#if TRIPLE_Z
, " Z3-Z2=", ABS(z_measured[2] - z_measured[1])
, " Z3-Z1=", ABS(z_measured[2] - z_measured[0])
#if QUAD_Z
, " Z4-Z3=", ABS(z_measured[3] - z_measured[2])
, " Z4-Z2=", ABS(z_measured[3] - z_measured[1])
, " Z4-Z1=", ABS(z_measured[3] - z_measured[0])
#endif
#endif
);

#if HAS_STATUS_MESSAGE
char fstr1[10];
#if NUM_Z_STEPPER_DRIVERS == 2
char msg[6 + (6 + 5) * 1 + 1];
#else
char msg[6 + (6 + 5) * 3 + 1], fstr2[10], fstr3[10];
#endif
sprintf_P(msg,
PSTR("Diffs Z1-Z2=%s"
#if NUM_Z_STEPPER_DRIVERS == 3
" Z2-Z3=%s"
" Z3-Z1=%s"
char msg[6 + (6 + 5) * NUM_Z_STEPPER_DRIVERS + 1]
#if TRIPLE_Z
, fstr2[10], fstr3[10]
#if QUAD_Z
, fstr4[10], fstr5[10], fstr6[10]
#endif
), dtostrf(ABS(z_measured[0] - z_measured[1]), 1, 3, fstr1)
#if NUM_Z_STEPPER_DRIVERS == 3
, dtostrf(ABS(z_measured[1] - z_measured[2]), 1, 3, fstr2)
, dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3)
#endif
;
sprintf_P(msg,
PSTR("1:2=%s" TERN_(TRIPLE_Z, " 3-2=%s 3-1=%s") TERN_(QUAD_Z, " 4-3=%s 4-2=%s 4-1=%s")),
dtostrf(ABS(z_measured[1] - z_measured[0]), 1, 3, fstr1)
OPTARG(TRIPLE_Z, dtostrf(ABS(z_measured[2] - z_measured[1]), 1, 3, fstr2))
OPTARG(TRIPLE_Z, dtostrf(ABS(z_measured[2] - z_measured[0]), 1, 3, fstr3))
OPTARG(QUAD_Z, dtostrf(ABS(z_measured[3] - z_measured[2]), 1, 3, fstr4))
OPTARG(QUAD_Z, dtostrf(ABS(z_measured[3] - z_measured[1]), 1, 3, fstr5))
OPTARG(QUAD_Z, dtostrf(ABS(z_measured[3] - z_measured[0]), 1, 3, fstr6))
);
ui.set_status(msg);
#endif

auto decreasing_accuracy = [](const_float_t v1, const_float_t v2){
auto decreasing_accuracy = [](const_float_t v1, const_float_t v2) {
if (v1 < v2 * 0.7f) {
SERIAL_ECHOLNPGM("Decreasing Accuracy Detected.");
LCD_MESSAGEPGM(MSG_DECREASING_ACCURACY);
Expand Down Expand Up @@ -437,7 +448,7 @@ void GcodeSuite::G34() {
#endif

}while(0);
#endif
#endif // Z_STEPPER_AUTO_ALIGN
}

#endif // Z_MULTI_ENDSTOPS || Z_STEPPER_AUTO_ALIGN
Expand Down

0 comments on commit c90fa53

Please sign in to comment.