|
@@ -23,93 +23,42 @@
|
23
|
23
|
|
24
|
24
|
#if ENABLED(AUTO_BED_LEVELING_UBL)
|
25
|
25
|
|
26
|
|
- #include "../bedlevel.h"
|
27
|
|
- #include "../../../module/planner.h"
|
28
|
|
- #include "../../../module/stepper.h"
|
29
|
|
- #include "../../../module/motion.h"
|
|
26
|
+#include "../bedlevel.h"
|
|
27
|
+#include "../../../module/planner.h"
|
|
28
|
+#include "../../../module/stepper.h"
|
|
29
|
+#include "../../../module/motion.h"
|
30
|
30
|
|
31
|
|
- #if ENABLED(DELTA)
|
32
|
|
- #include "../../../module/delta.h"
|
33
|
|
- #endif
|
34
|
|
-
|
35
|
|
- #include "../../../Marlin.h"
|
36
|
|
- #include <math.h>
|
37
|
|
-
|
38
|
|
- extern float destination[XYZE];
|
39
|
|
-
|
40
|
|
- #if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this
|
41
|
|
- inline void set_current_from_destination() { COPY(current_position, destination); }
|
42
|
|
- #else
|
43
|
|
- extern void set_current_from_destination();
|
44
|
|
- #endif
|
|
31
|
+#if ENABLED(DELTA)
|
|
32
|
+ #include "../../../module/delta.h"
|
|
33
|
+#endif
|
45
|
34
|
|
46
|
|
- static void debug_echo_axis(const AxisEnum axis) {
|
47
|
|
- if (current_position[axis] == destination[axis])
|
48
|
|
- SERIAL_ECHOPGM("-------------");
|
49
|
|
- else
|
50
|
|
- SERIAL_ECHO_F(destination[X_AXIS], 6);
|
51
|
|
- }
|
|
35
|
+#include "../../../Marlin.h"
|
|
36
|
+#include <math.h>
|
52
|
37
|
|
53
|
|
- void debug_current_and_destination(const char *title) {
|
54
|
|
-
|
55
|
|
- // if the title message starts with a '!' it is so important, we are going to
|
56
|
|
- // ignore the status of the g26_debug_flag
|
57
|
|
- if (*title != '!' && !g26_debug_flag) return;
|
58
|
|
-
|
59
|
|
- const float de = destination[E_AXIS] - current_position[E_AXIS];
|
60
|
|
-
|
61
|
|
- if (de == 0.0) return; // Printing moves only
|
62
|
|
-
|
63
|
|
- const float dx = destination[X_AXIS] - current_position[X_AXIS],
|
64
|
|
- dy = destination[Y_AXIS] - current_position[Y_AXIS],
|
65
|
|
- xy_dist = HYPOT(dx, dy);
|
66
|
|
-
|
67
|
|
- if (xy_dist == 0.0) return;
|
68
|
|
-
|
69
|
|
- SERIAL_ECHOPGM(" fpmm=");
|
70
|
|
- const float fpmm = de / xy_dist;
|
71
|
|
- SERIAL_ECHO_F(fpmm, 6);
|
72
|
|
-
|
73
|
|
- SERIAL_ECHOPGM(" current=( ");
|
74
|
|
- SERIAL_ECHO_F(current_position[X_AXIS], 6);
|
75
|
|
- SERIAL_ECHOPGM(", ");
|
76
|
|
- SERIAL_ECHO_F(current_position[Y_AXIS], 6);
|
77
|
|
- SERIAL_ECHOPGM(", ");
|
78
|
|
- SERIAL_ECHO_F(current_position[Z_AXIS], 6);
|
79
|
|
- SERIAL_ECHOPGM(", ");
|
80
|
|
- SERIAL_ECHO_F(current_position[E_AXIS], 6);
|
81
|
|
- SERIAL_ECHOPGM(" ) destination=( ");
|
82
|
|
- debug_echo_axis(X_AXIS);
|
83
|
|
- SERIAL_ECHOPGM(", ");
|
84
|
|
- debug_echo_axis(Y_AXIS);
|
85
|
|
- SERIAL_ECHOPGM(", ");
|
86
|
|
- debug_echo_axis(Z_AXIS);
|
87
|
|
- SERIAL_ECHOPGM(", ");
|
88
|
|
- debug_echo_axis(E_AXIS);
|
89
|
|
- SERIAL_ECHOPGM(" ) ");
|
90
|
|
- SERIAL_ECHO(title);
|
91
|
|
- SERIAL_EOL();
|
|
38
|
+#if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this
|
|
39
|
+ inline void set_current_from_destination() { COPY(current_position, destination); }
|
|
40
|
+#else
|
|
41
|
+ extern void set_current_from_destination();
|
|
42
|
+#endif
|
92
|
43
|
|
93
|
|
- }
|
|
44
|
+#if !UBL_SEGMENTED
|
94
|
45
|
|
95
|
|
- void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
|
|
46
|
+ void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const uint8_t extruder) {
|
96
|
47
|
/**
|
97
|
48
|
* Much of the nozzle movement will be within the same cell. So we will do as little computation
|
98
|
49
|
* as possible to determine if this is the case. If this move is within the same cell, we will
|
99
|
50
|
* just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
|
100
|
51
|
*/
|
101
|
|
- const float start[XYZE] = {
|
102
|
|
- current_position[X_AXIS],
|
103
|
|
- current_position[Y_AXIS],
|
104
|
|
- current_position[Z_AXIS],
|
105
|
|
- current_position[E_AXIS]
|
106
|
|
- },
|
107
|
|
- end[XYZE] = {
|
108
|
|
- destination[X_AXIS],
|
109
|
|
- destination[Y_AXIS],
|
110
|
|
- destination[Z_AXIS],
|
111
|
|
- destination[E_AXIS]
|
112
|
|
- };
|
|
52
|
+ #if ENABLED(SKEW_CORRECTION)
|
|
53
|
+ // For skew correction just adjust the destination point and we're done
|
|
54
|
+ float start[XYZE] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS] },
|
|
55
|
+ end[XYZE] = { destination[X_AXIS], destination[Y_AXIS], destination[Z_AXIS], destination[E_AXIS] };
|
|
56
|
+ planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]);
|
|
57
|
+ planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]);
|
|
58
|
+ #else
|
|
59
|
+ const float (&start)[XYZE] = current_position,
|
|
60
|
+ (&end)[XYZE] = destination;
|
|
61
|
+ #endif
|
113
|
62
|
|
114
|
63
|
const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
|
115
|
64
|
cell_start_yi = get_cell_index_y(start[Y_AXIS]),
|
|
@@ -117,13 +66,13 @@
|
117
|
66
|
cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
|
118
|
67
|
|
119
|
68
|
if (g26_debug_flag) {
|
120
|
|
- SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
|
121
|
|
- SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
|
122
|
|
- SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
|
123
|
|
- SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
|
|
69
|
+ SERIAL_ECHOPAIR(" ubl.line_to_destination_cartesian(xe=", destination[X_AXIS]);
|
|
70
|
+ SERIAL_ECHOPAIR(", ye=", destination[Y_AXIS]);
|
|
71
|
+ SERIAL_ECHOPAIR(", ze=", destination[Z_AXIS]);
|
|
72
|
+ SERIAL_ECHOPAIR(", ee=", destination[E_AXIS]);
|
124
|
73
|
SERIAL_CHAR(')');
|
125
|
74
|
SERIAL_EOL();
|
126
|
|
- debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
|
|
75
|
+ debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()"));
|
127
|
76
|
}
|
128
|
77
|
|
129
|
78
|
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
|
|
@@ -139,11 +88,11 @@
|
139
|
88
|
// Note: There is no Z Correction in this case. We are off the grid and don't know what
|
140
|
89
|
// a reasonable correction would be.
|
141
|
90
|
|
142
|
|
- planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
|
|
91
|
+ planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
|
143
|
92
|
set_current_from_destination();
|
144
|
93
|
|
145
|
94
|
if (g26_debug_flag)
|
146
|
|
- debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
|
|
95
|
+ debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination_cartesian()"));
|
147
|
96
|
|
148
|
97
|
return;
|
149
|
98
|
}
|
|
@@ -183,10 +132,10 @@
|
183
|
132
|
*/
|
184
|
133
|
if (isnan(z0)) z0 = 0.0;
|
185
|
134
|
|
186
|
|
- planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
|
|
135
|
+ planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
|
187
|
136
|
|
188
|
137
|
if (g26_debug_flag)
|
189
|
|
- debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
|
|
138
|
+ debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination_cartesian()"));
|
190
|
139
|
|
191
|
140
|
set_current_from_destination();
|
192
|
141
|
return;
|
|
@@ -274,7 +223,7 @@
|
274
|
223
|
* Without this check, it is possible for the algorithm to generate a zero length move in the case
|
275
|
224
|
* where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
|
276
|
225
|
* happens, it might be best to remove the check and always 'schedule' the move because
|
277
|
|
- * the planner._buffer_line() routine will filter it if that happens.
|
|
226
|
+ * the planner.buffer_segment() routine will filter it if that happens.
|
278
|
227
|
*/
|
279
|
228
|
if (ry != start[Y_AXIS]) {
|
280
|
229
|
if (!inf_normalized_flag) {
|
|
@@ -287,12 +236,12 @@
|
287
|
236
|
z_position = end[Z_AXIS];
|
288
|
237
|
}
|
289
|
238
|
|
290
|
|
- planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
|
|
239
|
+ planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
|
291
|
240
|
} //else printf("FIRST MOVE PRUNED ");
|
292
|
241
|
}
|
293
|
242
|
|
294
|
243
|
if (g26_debug_flag)
|
295
|
|
- debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
|
|
244
|
+ debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination_cartesian()"));
|
296
|
245
|
|
297
|
246
|
//
|
298
|
247
|
// Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
|
|
@@ -338,7 +287,7 @@
|
338
|
287
|
* Without this check, it is possible for the algorithm to generate a zero length move in the case
|
339
|
288
|
* where the line is heading left and it is starting right on a Mesh Line boundary. For how often
|
340
|
289
|
* that happens, it might be best to remove the check and always 'schedule' the move because
|
341
|
|
- * the planner._buffer_line() routine will filter it if that happens.
|
|
290
|
+ * the planner.buffer_segment() routine will filter it if that happens.
|
342
|
291
|
*/
|
343
|
292
|
if (rx != start[X_AXIS]) {
|
344
|
293
|
if (!inf_normalized_flag) {
|
|
@@ -351,12 +300,12 @@
|
351
|
300
|
z_position = end[Z_AXIS];
|
352
|
301
|
}
|
353
|
302
|
|
354
|
|
- planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
|
|
303
|
+ planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
|
355
|
304
|
} //else printf("FIRST MOVE PRUNED ");
|
356
|
305
|
}
|
357
|
306
|
|
358
|
307
|
if (g26_debug_flag)
|
359
|
|
- debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
|
|
308
|
+ debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination_cartesian()"));
|
360
|
309
|
|
361
|
310
|
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
|
362
|
311
|
goto FINAL_MOVE;
|
|
@@ -413,7 +362,7 @@
|
413
|
362
|
e_position = end[E_AXIS];
|
414
|
363
|
z_position = end[Z_AXIS];
|
415
|
364
|
}
|
416
|
|
- planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
|
|
365
|
+ planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
|
417
|
366
|
current_yi += dyi;
|
418
|
367
|
yi_cnt--;
|
419
|
368
|
}
|
|
@@ -441,7 +390,7 @@
|
441
|
390
|
z_position = end[Z_AXIS];
|
442
|
391
|
}
|
443
|
392
|
|
444
|
|
- planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
|
|
393
|
+ planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
|
445
|
394
|
current_xi += dxi;
|
446
|
395
|
xi_cnt--;
|
447
|
396
|
}
|
|
@@ -450,7 +399,7 @@
|
450
|
399
|
}
|
451
|
400
|
|
452
|
401
|
if (g26_debug_flag)
|
453
|
|
- debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
|
|
402
|
+ debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination_cartesian()"));
|
454
|
403
|
|
455
|
404
|
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
|
456
|
405
|
goto FINAL_MOVE;
|
|
@@ -458,223 +407,222 @@
|
458
|
407
|
set_current_from_destination();
|
459
|
408
|
}
|
460
|
409
|
|
461
|
|
- #if UBL_DELTA
|
|
410
|
+#else // UBL_SEGMENTED
|
462
|
411
|
|
463
|
|
- // macro to inline copy exactly 4 floats, don't rely on sizeof operator
|
464
|
|
- #define COPY_XYZE( target, source ) { \
|
465
|
|
- target[X_AXIS] = source[X_AXIS]; \
|
466
|
|
- target[Y_AXIS] = source[Y_AXIS]; \
|
467
|
|
- target[Z_AXIS] = source[Z_AXIS]; \
|
468
|
|
- target[E_AXIS] = source[E_AXIS]; \
|
469
|
|
- }
|
|
412
|
+ #if IS_SCARA // scale the feed rate from mm/s to degrees/s
|
|
413
|
+ static float scara_feed_factor, scara_oldA, scara_oldB;
|
|
414
|
+ #endif
|
470
|
415
|
|
471
|
|
- #if IS_SCARA // scale the feed rate from mm/s to degrees/s
|
472
|
|
- static float scara_feed_factor, scara_oldA, scara_oldB;
|
473
|
|
- #endif
|
|
416
|
+ // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
|
|
417
|
+ // so we call buffer_segment directly here. Per-segmented leveling and kinematics performed first.
|
474
|
418
|
|
475
|
|
- // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
|
476
|
|
- // so we call _buffer_line directly here. Per-segmented leveling and kinematics performed first.
|
|
419
|
+ inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) {
|
477
|
420
|
|
478
|
|
- inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) {
|
|
421
|
+ #if ENABLED(SKEW_CORRECTION)
|
|
422
|
+ float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] };
|
|
423
|
+ planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]);
|
|
424
|
+ #else
|
|
425
|
+ const float (&raw)[XYZE] = in_raw;
|
|
426
|
+ #endif
|
479
|
427
|
|
480
|
|
- #if ENABLED(DELTA) // apply delta inverse_kinematics
|
|
428
|
+ #if ENABLED(DELTA) // apply delta inverse_kinematics
|
481
|
429
|
|
482
|
|
- DELTA_RAW_IK();
|
483
|
|
- planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
|
|
430
|
+ DELTA_RAW_IK();
|
|
431
|
+ planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder);
|
484
|
432
|
|
485
|
|
- #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
|
|
433
|
+ #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
|
486
|
434
|
|
487
|
|
- inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
|
488
|
|
- // should move the feedrate scaling to scara inverse_kinematics
|
|
435
|
+ inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
|
|
436
|
+ // should move the feedrate scaling to scara inverse_kinematics
|
489
|
437
|
|
490
|
|
- const float adiff = FABS(delta[A_AXIS] - scara_oldA),
|
491
|
|
- bdiff = FABS(delta[B_AXIS] - scara_oldB);
|
492
|
|
- scara_oldA = delta[A_AXIS];
|
493
|
|
- scara_oldB = delta[B_AXIS];
|
494
|
|
- float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
|
|
438
|
+ const float adiff = FABS(delta[A_AXIS] - scara_oldA),
|
|
439
|
+ bdiff = FABS(delta[B_AXIS] - scara_oldB);
|
|
440
|
+ scara_oldA = delta[A_AXIS];
|
|
441
|
+ scara_oldB = delta[B_AXIS];
|
|
442
|
+ float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
|
495
|
443
|
|
496
|
|
- planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder);
|
|
444
|
+ planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder);
|
497
|
445
|
|
498
|
|
- #else // CARTESIAN
|
|
446
|
+ #else // CARTESIAN
|
499
|
447
|
|
500
|
|
- planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder);
|
|
448
|
+ planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder);
|
501
|
449
|
|
502
|
|
- #endif
|
503
|
|
- }
|
|
450
|
+ #endif
|
|
451
|
+ }
|
504
|
452
|
|
505
|
|
- #if IS_SCARA
|
506
|
|
- #define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm
|
507
|
|
- #elif ENABLED(DELTA)
|
508
|
|
- #define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND)
|
509
|
|
- #else // CARTESIAN
|
510
|
|
- #ifdef LEVELED_SEGMENT_LENGTH
|
511
|
|
- #define DELTA_SEGMENT_MIN_LENGTH LEVELED_SEGMENT_LENGTH
|
512
|
|
- #else
|
513
|
|
- #define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation)
|
514
|
|
- #endif
|
|
453
|
+ #if IS_SCARA
|
|
454
|
+ #define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm
|
|
455
|
+ #elif ENABLED(DELTA)
|
|
456
|
+ #define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND)
|
|
457
|
+ #else // CARTESIAN
|
|
458
|
+ #ifdef LEVELED_SEGMENT_LENGTH
|
|
459
|
+ #define DELTA_SEGMENT_MIN_LENGTH LEVELED_SEGMENT_LENGTH
|
|
460
|
+ #else
|
|
461
|
+ #define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation)
|
515
|
462
|
#endif
|
|
463
|
+ #endif
|
516
|
464
|
|
517
|
|
- /**
|
518
|
|
- * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
|
519
|
|
- * This calls planner._buffer_line multiple times for small incremental moves.
|
520
|
|
- * Returns true if did NOT move, false if moved (requires current_position update).
|
521
|
|
- */
|
|
465
|
+ /**
|
|
466
|
+ * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics.
|
|
467
|
+ * This calls planner.buffer_segment multiple times for small incremental moves.
|
|
468
|
+ * Returns true if did NOT move, false if moved (requires current_position update).
|
|
469
|
+ */
|
|
470
|
+
|
|
471
|
+ bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) {
|
|
472
|
+
|
|
473
|
+ if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
|
|
474
|
+ return true; // did not move, so current_position still accurate
|
|
475
|
+
|
|
476
|
+ const float total[XYZE] = {
|
|
477
|
+ rtarget[X_AXIS] - current_position[X_AXIS],
|
|
478
|
+ rtarget[Y_AXIS] - current_position[Y_AXIS],
|
|
479
|
+ rtarget[Z_AXIS] - current_position[Z_AXIS],
|
|
480
|
+ rtarget[E_AXIS] - current_position[E_AXIS]
|
|
481
|
+ };
|
|
482
|
+
|
|
483
|
+ const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance
|
|
484
|
+
|
|
485
|
+ #if IS_KINEMATIC
|
|
486
|
+ const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
|
|
487
|
+ uint16_t segments = lroundf(delta_segments_per_second * seconds), // preferred number of segments for distance @ feedrate
|
|
488
|
+ seglimit = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
|
|
489
|
+ NOMORE(segments, seglimit); // limit to minimum segment length (fewer segments)
|
|
490
|
+ #else
|
|
491
|
+ uint16_t segments = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
|
|
492
|
+ #endif
|
522
|
493
|
|
523
|
|
- bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate) {
|
524
|
|
-
|
525
|
|
- if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
|
526
|
|
- return true; // did not move, so current_position still accurate
|
527
|
|
-
|
528
|
|
- const float total[XYZE] = {
|
529
|
|
- rtarget[X_AXIS] - current_position[X_AXIS],
|
530
|
|
- rtarget[Y_AXIS] - current_position[Y_AXIS],
|
531
|
|
- rtarget[Z_AXIS] - current_position[Z_AXIS],
|
532
|
|
- rtarget[E_AXIS] - current_position[E_AXIS]
|
533
|
|
- };
|
534
|
|
-
|
535
|
|
- const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance
|
536
|
|
-
|
537
|
|
- #if IS_KINEMATIC
|
538
|
|
- const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
|
539
|
|
- uint16_t segments = lroundf(delta_segments_per_second * seconds), // preferred number of segments for distance @ feedrate
|
540
|
|
- seglimit = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length
|
541
|
|
- NOMORE(segments, seglimit); // limit to minimum segment length (fewer segments)
|
542
|
|
- #else
|
543
|
|
- uint16_t segments = lroundf(cartesian_xy_mm * (1.0 / (DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length
|
544
|
|
- #endif
|
545
|
|
-
|
546
|
|
- NOLESS(segments, 1); // must have at least one segment
|
547
|
|
- const float inv_segments = 1.0 / segments; // divide once, multiply thereafter
|
548
|
|
-
|
549
|
|
- #if IS_SCARA // scale the feed rate from mm/s to degrees/s
|
550
|
|
- scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
|
551
|
|
- scara_oldA = stepper.get_axis_position_degrees(A_AXIS);
|
552
|
|
- scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
|
553
|
|
- #endif
|
554
|
|
-
|
555
|
|
- const float diff[XYZE] = {
|
556
|
|
- total[X_AXIS] * inv_segments,
|
557
|
|
- total[Y_AXIS] * inv_segments,
|
558
|
|
- total[Z_AXIS] * inv_segments,
|
559
|
|
- total[E_AXIS] * inv_segments
|
560
|
|
- };
|
561
|
|
-
|
562
|
|
- // Note that E segment distance could vary slightly as z mesh height
|
563
|
|
- // changes for each segment, but small enough to ignore.
|
564
|
|
-
|
565
|
|
- float raw[XYZE] = {
|
566
|
|
- current_position[X_AXIS],
|
567
|
|
- current_position[Y_AXIS],
|
568
|
|
- current_position[Z_AXIS],
|
569
|
|
- current_position[E_AXIS]
|
570
|
|
- };
|
571
|
|
-
|
572
|
|
- // Only compute leveling per segment if ubl active and target below z_fade_height.
|
573
|
|
- if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
|
574
|
|
- while (--segments) {
|
575
|
|
- LOOP_XYZE(i) raw[i] += diff[i];
|
576
|
|
- ubl_buffer_segment_raw(raw, feedrate);
|
577
|
|
- }
|
578
|
|
- ubl_buffer_segment_raw(rtarget, feedrate);
|
579
|
|
- return false; // moved but did not set_current_from_destination();
|
|
494
|
+ NOLESS(segments, 1); // must have at least one segment
|
|
495
|
+ const float inv_segments = 1.0 / segments; // divide once, multiply thereafter
|
|
496
|
+
|
|
497
|
+ #if IS_SCARA // scale the feed rate from mm/s to degrees/s
|
|
498
|
+ scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate;
|
|
499
|
+ scara_oldA = stepper.get_axis_position_degrees(A_AXIS);
|
|
500
|
+ scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
|
|
501
|
+ #endif
|
|
502
|
+
|
|
503
|
+ const float diff[XYZE] = {
|
|
504
|
+ total[X_AXIS] * inv_segments,
|
|
505
|
+ total[Y_AXIS] * inv_segments,
|
|
506
|
+ total[Z_AXIS] * inv_segments,
|
|
507
|
+ total[E_AXIS] * inv_segments
|
|
508
|
+ };
|
|
509
|
+
|
|
510
|
+ // Note that E segment distance could vary slightly as z mesh height
|
|
511
|
+ // changes for each segment, but small enough to ignore.
|
|
512
|
+
|
|
513
|
+ float raw[XYZE] = {
|
|
514
|
+ current_position[X_AXIS],
|
|
515
|
+ current_position[Y_AXIS],
|
|
516
|
+ current_position[Z_AXIS],
|
|
517
|
+ current_position[E_AXIS]
|
|
518
|
+ };
|
|
519
|
+
|
|
520
|
+ // Only compute leveling per segment if ubl active and target below z_fade_height.
|
|
521
|
+ if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
|
|
522
|
+ while (--segments) {
|
|
523
|
+ LOOP_XYZE(i) raw[i] += diff[i];
|
|
524
|
+ ubl_buffer_segment_raw(raw, feedrate);
|
580
|
525
|
}
|
|
526
|
+ ubl_buffer_segment_raw(rtarget, feedrate);
|
|
527
|
+ return false; // moved but did not set_current_from_destination();
|
|
528
|
+ }
|
581
|
529
|
|
582
|
|
- // Otherwise perform per-segment leveling
|
|
530
|
+ // Otherwise perform per-segment leveling
|
583
|
531
|
|
584
|
|
- #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
585
|
|
- const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
|
586
|
|
- #endif
|
|
532
|
+ #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
|
533
|
+ const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
|
|
534
|
+ #endif
|
587
|
535
|
|
588
|
|
- // increment to first segment destination
|
589
|
|
- LOOP_XYZE(i) raw[i] += diff[i];
|
|
536
|
+ // increment to first segment destination
|
|
537
|
+ LOOP_XYZE(i) raw[i] += diff[i];
|
590
|
538
|
|
591
|
|
- for(;;) { // for each mesh cell encountered during the move
|
|
539
|
+ for(;;) { // for each mesh cell encountered during the move
|
592
|
540
|
|
593
|
|
- // Compute mesh cell invariants that remain constant for all segments within cell.
|
594
|
|
- // Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
|
595
|
|
- // the bilinear interpolation from the adjacent cell within the mesh will still work.
|
596
|
|
- // Inner loop will exit each time (because out of cell bounds) but will come back
|
597
|
|
- // in top of loop and again re-find same adjacent cell and use it, just less efficient
|
598
|
|
- // for mesh inset area.
|
|
541
|
+ // Compute mesh cell invariants that remain constant for all segments within cell.
|
|
542
|
+ // Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter)
|
|
543
|
+ // the bilinear interpolation from the adjacent cell within the mesh will still work.
|
|
544
|
+ // Inner loop will exit each time (because out of cell bounds) but will come back
|
|
545
|
+ // in top of loop and again re-find same adjacent cell and use it, just less efficient
|
|
546
|
+ // for mesh inset area.
|
599
|
547
|
|
600
|
|
- int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
|
601
|
|
- cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
|
|
548
|
+ int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
|
|
549
|
+ cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
|
602
|
550
|
|
603
|
|
- cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
|
604
|
|
- cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
|
|
551
|
+ cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
|
|
552
|
+ cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
|
605
|
553
|
|
606
|
|
- const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add
|
607
|
|
- y0 = mesh_index_to_ypos(cell_yi);
|
|
554
|
+ const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add
|
|
555
|
+ y0 = mesh_index_to_ypos(cell_yi);
|
608
|
556
|
|
609
|
|
- float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
|
610
|
|
- z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner
|
611
|
|
- z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner
|
612
|
|
- z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner
|
|
557
|
+ float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner
|
|
558
|
+ z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner
|
|
559
|
+ z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner
|
|
560
|
+ z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner
|
613
|
561
|
|
614
|
|
- if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating planner.leveling_active (G29 A)
|
615
|
|
- if (isnan(z_x1y0)) z_x1y0 = 0; // should refuse if any invalid mesh points
|
616
|
|
- if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
|
617
|
|
- if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
|
|
562
|
+ if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating planner.leveling_active (G29 A)
|
|
563
|
+ if (isnan(z_x1y0)) z_x1y0 = 0; // should refuse if any invalid mesh points
|
|
564
|
+ if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
|
|
565
|
+ if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
|
618
|
566
|
|
619
|
|
- float cx = raw[X_AXIS] - x0, // cell-relative x and y
|
620
|
|
- cy = raw[Y_AXIS] - y0;
|
|
567
|
+ float cx = raw[X_AXIS] - x0, // cell-relative x and y
|
|
568
|
+ cy = raw[Y_AXIS] - y0;
|
621
|
569
|
|
622
|
|
- const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right)
|
623
|
|
- z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right)
|
|
570
|
+ const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right)
|
|
571
|
+ z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right)
|
624
|
572
|
|
625
|
|
- float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx (changes for each cx in cell)
|
|
573
|
+ float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx (changes for each cx in cell)
|
626
|
574
|
|
627
|
|
- const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx
|
628
|
|
- z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1
|
|
575
|
+ const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx
|
|
576
|
+ z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1
|
629
|
577
|
|
630
|
|
- float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 (changes for each cx in cell)
|
|
578
|
+ float z_cxym = z_cxyd * (1.0 / (MESH_Y_DIST)); // z slope per y along cx from y0 to y1 (changes for each cx in cell)
|
631
|
579
|
|
632
|
|
- // float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop)
|
|
580
|
+ // float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop)
|
633
|
581
|
|
634
|
|
- // As subsequent segments step through this cell, the z_cxy0 intercept will change
|
635
|
|
- // and the z_cxym slope will change, both as a function of cx within the cell, and
|
636
|
|
- // each change by a constant for fixed segment lengths.
|
|
582
|
+ // As subsequent segments step through this cell, the z_cxy0 intercept will change
|
|
583
|
+ // and the z_cxym slope will change, both as a function of cx within the cell, and
|
|
584
|
+ // each change by a constant for fixed segment lengths.
|
637
|
585
|
|
638
|
|
- const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0
|
639
|
|
- z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym
|
|
586
|
+ const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0
|
|
587
|
+ z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym
|
640
|
588
|
|
641
|
|
- for(;;) { // for all segments within this mesh cell
|
|
589
|
+ for(;;) { // for all segments within this mesh cell
|
642
|
590
|
|
643
|
|
- if (--segments == 0) // if this is last segment, use rtarget for exact
|
644
|
|
- COPY(raw, rtarget);
|
|
591
|
+ if (--segments == 0) // if this is last segment, use rtarget for exact
|
|
592
|
+ COPY(raw, rtarget);
|
645
|
593
|
|
646
|
|
- const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
|
647
|
|
- #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
648
|
|
- * fade_scaling_factor // apply fade factor to interpolated mesh height
|
649
|
|
- #endif
|
650
|
|
- ;
|
|
594
|
+ const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
|
|
595
|
+ #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
|
596
|
+ * fade_scaling_factor // apply fade factor to interpolated mesh height
|
|
597
|
+ #endif
|
|
598
|
+ ;
|
651
|
599
|
|
652
|
|
- const float z = raw[Z_AXIS];
|
653
|
|
- raw[Z_AXIS] += z_cxcy;
|
654
|
|
- ubl_buffer_segment_raw(raw, feedrate);
|
655
|
|
- raw[Z_AXIS] = z;
|
|
600
|
+ const float z = raw[Z_AXIS];
|
|
601
|
+ raw[Z_AXIS] += z_cxcy;
|
|
602
|
+ ubl_buffer_segment_raw(raw, feedrate);
|
|
603
|
+ raw[Z_AXIS] = z;
|
656
|
604
|
|
657
|
|
- if (segments == 0) // done with last segment
|
658
|
|
- return false; // did not set_current_from_destination()
|
|
605
|
+ if (segments == 0) // done with last segment
|
|
606
|
+ return false; // did not set_current_from_destination()
|
659
|
607
|
|
660
|
|
- LOOP_XYZE(i) raw[i] += diff[i];
|
|
608
|
+ LOOP_XYZE(i) raw[i] += diff[i];
|
661
|
609
|
|
662
|
|
- cx += diff[X_AXIS];
|
663
|
|
- cy += diff[Y_AXIS];
|
|
610
|
+ cx += diff[X_AXIS];
|
|
611
|
+ cy += diff[Y_AXIS];
|
664
|
612
|
|
665
|
|
- if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next
|
666
|
|
- break;
|
|
613
|
+ if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next
|
|
614
|
+ break;
|
667
|
615
|
|
668
|
|
- // Next segment still within same mesh cell, adjust the per-segment
|
669
|
|
- // slope and intercept to compute next z height.
|
|
616
|
+ // Next segment still within same mesh cell, adjust the per-segment
|
|
617
|
+ // slope and intercept to compute next z height.
|
670
|
618
|
|
671
|
|
- z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0
|
672
|
|
- z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym
|
|
619
|
+ z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0
|
|
620
|
+ z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym
|
673
|
621
|
|
674
|
|
- } // segment loop
|
675
|
|
- } // cell loop
|
676
|
|
- }
|
|
622
|
+ } // segment loop
|
|
623
|
+ } // cell loop
|
|
624
|
+ }
|
677
|
625
|
|
678
|
|
- #endif // UBL_DELTA
|
|
626
|
+#endif // UBL_SEGMENTED
|
679
|
627
|
|
680
|
628
|
#endif // AUTO_BED_LEVELING_UBL
|