Tweak G34, fix a declaration

This commit is contained in:
Scott Lahteine 2019-12-19 00:39:09 -06:00
parent 768a1d2713
commit 0400783444

View File

@ -61,13 +61,16 @@ constexpr xy_pos_t sanity_arr_z_align[] = Z_STEPPER_ALIGN_XY;
); );
#endif #endif
static xy_pos_t z_auto_align_pos[Z_STEPPER_COUNT] = Z_STEPPER_ALIGN_XY; //
// G34 / M422 shared data
//
static xy_pos_t z_stepper_align_pos[] = Z_STEPPER_ALIGN_XY;
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
static xy_pos_t z_stepper_pos[] = Z_STEPPER_ALIGN_STEPPER_XY; static xy_pos_t z_stepper_align_stepper_pos[] = Z_STEPPER_ALIGN_STEPPER_XY;
#endif #endif
#define G34_PROBE_COUNT COUNT(z_auto_align_pos) #define G34_PROBE_COUNT COUNT(z_stepper_align_pos)
inline void set_all_z_lock(const bool lock) { inline void set_all_z_lock(const bool lock) {
stepper.set_z_lock(lock); stepper.set_z_lock(lock);
@ -155,11 +158,11 @@ void GcodeSuite::G34() {
// iteration this will be re-calculated based on the actual bed position // iteration this will be re-calculated based on the actual bed position
float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * ( float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * (
#if ENABLED(Z_TRIPLE_STEPPER_DRIVERS) #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), 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_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_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_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_stepper_align_pos[2].x - z_stepper_align_pos[2].y, z_stepper_align_pos[0].x - z_stepper_align_pos[0].y)))
#else #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) 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)
#endif #endif
); );
@ -194,7 +197,7 @@ void GcodeSuite::G34() {
if (iteration == 0 || i > 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. // Probe a Z height for each stepper.
const float z_probed_height = probe_at_point(z_auto_align_pos[iprobe], raise_after, 0, true); const float z_probed_height = probe_at_point(z_stepper_align_pos[iprobe], raise_after, 0, true);
if (isnan(z_probed_height)) { if (isnan(z_probed_height)) {
SERIAL_ECHOLNPGM("Probing failed."); SERIAL_ECHOLNPGM("Probing failed.");
err_break = true; err_break = true;
@ -235,13 +238,13 @@ void GcodeSuite::G34() {
incremental_LSF_reset(&lfd); incremental_LSF_reset(&lfd);
for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) { for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) {
SERIAL_ECHOLNPAIR("PROBEPT_", int(i + 1), ": ", z_measured[i]); SERIAL_ECHOLNPAIR("PROBEPT_", int(i + 1), ": ", z_measured[i]);
incremental_LSF(&lfd, z_auto_align_pos[i], z_measured[i]); incremental_LSF(&lfd, z_stepper_align_pos[i], z_measured[i]);
} }
finish_incremental_LSF(&lfd); finish_incremental_LSF(&lfd);
z_measured_min = 100000.0f; z_measured_min = 100000.0f;
for (uint8_t i = 0; i < Z_STEPPER_COUNT; ++i) { 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[i] = -(lfd.A * z_stepper_align_stepper_pos[i].x + lfd.B * z_stepper_align_stepper_pos[i].y);
z_measured_min = _MIN(z_measured_min, z_measured[i]); z_measured_min = _MIN(z_measured_min, z_measured[i]);
} }
@ -362,10 +365,10 @@ void GcodeSuite::G34() {
void GcodeSuite::M422() { void GcodeSuite::M422() {
if (!parser.seen_any()) { if (!parser.seen_any()) {
for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i) for (uint8_t i = 0; i < G34_PROBE_COUNT; ++i)
SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), i + 1, SP_X_STR, z_auto_align_pos[i].x, SP_Y_STR, z_auto_align_pos[i].y); 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);
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
for (uint8_t i = 0; i < Z_STEPPER_COUNT; ++i) for (uint8_t i = 0; i < Z_STEPPER_COUNT; ++i)
SERIAL_ECHOLNPAIR_P(PSTR("M422 W"), i + 1, SP_X_STR, z_stepper_pos[i].x, SP_Y_STR, z_stepper_pos[i].y); 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);
#endif #endif
return; return;
} }
@ -381,9 +384,9 @@ void GcodeSuite::M422() {
xy_pos_t *pos_dest = ( xy_pos_t *pos_dest = (
#if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS) #if ENABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
!is_probe_point ? z_stepper_pos : !is_probe_point ? z_stepper_align_stepper_pos :
#endif #endif
z_auto_align_pos z_stepper_align_pos
); );
if (!is_probe_point if (!is_probe_point