|
@@ -64,6 +64,7 @@ inline void set_all_z_lock(const bool lock) {
|
64
|
64
|
* I<iterations>
|
65
|
65
|
* T<accuracy>
|
66
|
66
|
* A<amplification>
|
|
67
|
+ * R<recalculate> points based on current probe offsets
|
67
|
68
|
*/
|
68
|
69
|
void GcodeSuite::G34() {
|
69
|
70
|
if (DEBUGGING(LEVELING)) {
|
|
@@ -73,8 +74,10 @@ void GcodeSuite::G34() {
|
73
|
74
|
|
74
|
75
|
do { // break out on error
|
75
|
76
|
|
76
|
|
- #if NUM_Z_STEPPER_DRIVERS >= 4
|
77
|
|
- SERIAL_ECHOLNPGM("Alignment not supported for over 3 steppers");
|
|
77
|
+ #if NUM_Z_STEPPER_DRIVERS == 4
|
|
78
|
+ SERIAL_ECHOLNPGM("Alignment for 4 steppers is Experimental!");
|
|
79
|
+ #elif NUM_Z_STEPPER_DRIVERS > 4
|
|
80
|
+ SERIAL_ECHOLNPGM("Alignment not supported for over 4 steppers");
|
78
|
81
|
break;
|
79
|
82
|
#endif
|
80
|
83
|
|
|
@@ -101,6 +104,8 @@ void GcodeSuite::G34() {
|
101
|
104
|
}
|
102
|
105
|
#endif
|
103
|
106
|
|
|
107
|
+ if (parser.seen('R')) z_stepper_align.reset_to_default();
|
|
108
|
+
|
104
|
109
|
const ProbePtRaise raise_after = parser.boolval('E') ? PROBE_PT_STOW : PROBE_PT_RAISE;
|
105
|
110
|
|
106
|
111
|
// Wait for planner moves to finish!
|
|
@@ -139,13 +144,18 @@ void GcodeSuite::G34() {
|
139
|
144
|
|
140
|
145
|
// Compute a worst-case clearance height to probe from. After the first
|
141
|
146
|
// iteration this will be re-calculated based on the actual bed position
|
142
|
|
- float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * (
|
|
147
|
+ auto magnitude2 = [&](const uint8_t i, const uint8_t j) {
|
|
148
|
+ const xy_pos_t diff = z_stepper_align.xy[i] - z_stepper_align.xy[j];
|
|
149
|
+ return HYPOT2(diff.x, diff.y);
|
|
150
|
+ };
|
|
151
|
+ float z_probe = Z_BASIC_CLEARANCE + (G34_MAX_GRADE) * 0.01f * SQRT(
|
143
|
152
|
#if NUM_Z_STEPPER_DRIVERS == 3
|
144
|
|
- SQRT(_MAX(HYPOT2(z_stepper_align.xy[0].x - z_stepper_align.xy[1].x, z_stepper_align.xy[0].y - z_stepper_align.xy[1].y),
|
145
|
|
- HYPOT2(z_stepper_align.xy[1].x - z_stepper_align.xy[2].x, z_stepper_align.xy[1].y - z_stepper_align.xy[2].y),
|
146
|
|
- HYPOT2(z_stepper_align.xy[2].x - z_stepper_align.xy[0].x, z_stepper_align.xy[2].y - z_stepper_align.xy[0].y)))
|
|
153
|
+ _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 0))
|
|
154
|
+ #elif NUM_Z_STEPPER_DRIVERS == 4
|
|
155
|
+ _MAX(magnitude2(0, 1), magnitude2(1, 2), magnitude2(2, 3),
|
|
156
|
+ magnitude2(3, 0), magnitude2(0, 2), magnitude2(1, 3))
|
147
|
157
|
#else
|
148
|
|
- HYPOT(z_stepper_align.xy[0].x - z_stepper_align.xy[1].x, z_stepper_align.xy[0].y - z_stepper_align.xy[1].y)
|
|
158
|
+ magnitude2(0, 1)
|
149
|
159
|
#endif
|
150
|
160
|
);
|
151
|
161
|
|
|
@@ -159,7 +169,7 @@ void GcodeSuite::G34() {
|
159
|
169
|
// This hack is un-done at the end of G34 - either by re-homing, or by using the probed heights of the last iteration.
|
160
|
170
|
|
161
|
171
|
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
|
162
|
|
- float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f);
|
|
172
|
+ float last_z_align_move[NUM_Z_STEPPER_DRIVERS] = ARRAY_N(NUM_Z_STEPPER_DRIVERS, 10000.0f, 10000.0f, 10000.0f, 10000.0f);
|
163
|
173
|
#else
|
164
|
174
|
float last_z_align_level_indicator = 10000.0f;
|
165
|
175
|
#endif
|
|
@@ -241,7 +251,7 @@ void GcodeSuite::G34() {
|
241
|
251
|
linear_fit_data lfd;
|
242
|
252
|
incremental_LSF_reset(&lfd);
|
243
|
253
|
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS) {
|
244
|
|
- SERIAL_ECHOLNPAIR("PROBEPT_", ('0' + i), ": ", z_measured[i]);
|
|
254
|
+ SERIAL_ECHOLNPAIR("PROBEPT_", int(i), ": ", z_measured[i]);
|
245
|
255
|
incremental_LSF(&lfd, z_stepper_align.xy[i], z_measured[i]);
|
246
|
256
|
}
|
247
|
257
|
finish_incremental_LSF(&lfd);
|
|
@@ -322,9 +332,12 @@ void GcodeSuite::G34() {
|
322
|
332
|
switch (zstepper) {
|
323
|
333
|
case 0: stepper.set_z_lock(false); break;
|
324
|
334
|
case 1: stepper.set_z2_lock(false); break;
|
325
|
|
- #if NUM_Z_STEPPER_DRIVERS == 3
|
|
335
|
+ #if NUM_Z_STEPPER_DRIVERS >= 3
|
326
|
336
|
case 2: stepper.set_z3_lock(false); break;
|
327
|
337
|
#endif
|
|
338
|
+ #if NUM_Z_STEPPER_DRIVERS == 4
|
|
339
|
+ case 3: stepper.set_z4_lock(false); break;
|
|
340
|
+ #endif
|
328
|
341
|
}
|
329
|
342
|
|
330
|
343
|
#if DISABLED(Z_STEPPER_ALIGN_KNOWN_STEPPER_POSITIONS)
|
|
@@ -348,28 +361,13 @@ void GcodeSuite::G34() {
|
348
|
361
|
|
349
|
362
|
} // for (iteration)
|
350
|
363
|
|
351
|
|
- if (err_break) {
|
|
364
|
+ if (err_break)
|
352
|
365
|
SERIAL_ECHOLNPGM("G34 aborted.");
|
353
|
|
- set_axis_not_trusted(Z_AXIS); // The Z coordinate is messed up now
|
354
|
|
- break;
|
|
366
|
+ else {
|
|
367
|
+ SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " of ", int(z_auto_align_iterations));
|
|
368
|
+ SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff);
|
355
|
369
|
}
|
356
|
370
|
|
357
|
|
- SERIAL_ECHOLNPAIR("Did ", int(iteration + (iteration != z_auto_align_iterations)), " iterations of ", int(z_auto_align_iterations));
|
358
|
|
- SERIAL_ECHOLNPAIR_F("Accuracy: ", z_maxdiff);
|
359
|
|
-
|
360
|
|
- // Restore the active tool after homing
|
361
|
|
- #if HOTENDS > 1
|
362
|
|
- tool_change(old_tool_index, (true
|
363
|
|
- #if ENABLED(PARKING_EXTRUDER)
|
364
|
|
- && false // Fetch the previous toolhead
|
365
|
|
- #endif
|
366
|
|
- ));
|
367
|
|
- #endif
|
368
|
|
-
|
369
|
|
- #if HAS_LEVELING && ENABLED(RESTORE_LEVELING_AFTER_G34)
|
370
|
|
- set_bed_leveling_enabled(leveling_was_active);
|
371
|
|
- #endif
|
372
|
|
-
|
373
|
371
|
// Stow the probe, as the last call to probe.probe_at_point(...) left
|
374
|
372
|
// the probe deployed if it was successful.
|
375
|
373
|
probe.stow();
|
|
@@ -377,7 +375,6 @@ void GcodeSuite::G34() {
|
377
|
375
|
#if ENABLED(HOME_AFTER_G34)
|
378
|
376
|
// After this operation the z position needs correction
|
379
|
377
|
set_axis_not_trusted(Z_AXIS);
|
380
|
|
-
|
381
|
378
|
// Home Z after the alignment procedure
|
382
|
379
|
process_subcommands_now_P(PSTR("G28Z"));
|
383
|
380
|
#else
|
|
@@ -388,6 +385,15 @@ void GcodeSuite::G34() {
|
388
|
385
|
sync_plan_position();
|
389
|
386
|
#endif
|
390
|
387
|
|
|
388
|
+ // Restore the active tool after homing
|
|
389
|
+ #if HOTENDS > 1
|
|
390
|
+ tool_change(old_tool_index, DISABLED(PARKING_EXTRUDER)); // Fetch previous tool for parking extruder
|
|
391
|
+ #endif
|
|
392
|
+
|
|
393
|
+ #if HAS_LEVELING && ENABLED(RESTORE_LEVELING_AFTER_G34)
|
|
394
|
+ set_bed_leveling_enabled(leveling_was_active);
|
|
395
|
+ #endif
|
|
396
|
+
|
391
|
397
|
}while(0);
|
392
|
398
|
|
393
|
399
|
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("<<< G34");
|
|
@@ -406,9 +412,16 @@ void GcodeSuite::G34() {
|
406
|
412
|
* S and W require an X and/or Y parameter
|
407
|
413
|
* X<pos> : X position to set (Unchanged if omitted)
|
408
|
414
|
* Y<pos> : Y position to set (Unchanged if omitted)
|
|
415
|
+ *
|
|
416
|
+ * R : Recalculate points based on current probe offsets
|
409
|
417
|
*/
|
410
|
418
|
void GcodeSuite::M422() {
|
411
|
419
|
|
|
420
|
+ if (parser.seen('R')) {
|
|
421
|
+ z_stepper_align.reset_to_default();
|
|
422
|
+ return;
|
|
423
|
+ }
|
|
424
|
+
|
412
|
425
|
if (!parser.seen_any()) {
|
413
|
426
|
LOOP_L_N(i, NUM_Z_STEPPER_DRIVERS)
|
414
|
427
|
SERIAL_ECHOLNPAIR_P(PSTR("M422 S"), int(i + 1), SP_X_STR, z_stepper_align.xy[i].x, SP_Y_STR, z_stepper_align.xy[i].y);
|