|
@@ -38,25 +38,6 @@
|
38
|
38
|
extern void set_current_from_destination();
|
39
|
39
|
#endif
|
40
|
40
|
|
41
|
|
- #if ENABLED(DELTA)
|
42
|
|
-
|
43
|
|
- extern float delta[ABC];
|
44
|
|
-
|
45
|
|
- extern float delta_endstop_adj[ABC],
|
46
|
|
- delta_radius,
|
47
|
|
- delta_tower_angle_trim[ABC],
|
48
|
|
- delta_tower[ABC][2],
|
49
|
|
- delta_diagonal_rod,
|
50
|
|
- delta_calibration_radius,
|
51
|
|
- delta_diagonal_rod_2_tower[ABC],
|
52
|
|
- delta_segments_per_second,
|
53
|
|
- delta_clip_start_height;
|
54
|
|
-
|
55
|
|
- extern float delta_safe_distance_from_top();
|
56
|
|
-
|
57
|
|
- #endif
|
58
|
|
-
|
59
|
|
-
|
60
|
41
|
static void debug_echo_axis(const AxisEnum axis) {
|
61
|
42
|
if (current_position[axis] == destination[axis])
|
62
|
43
|
SERIAL_ECHOPGM("-------------");
|
|
@@ -268,9 +249,9 @@
|
268
|
249
|
* else, we know the next X is the same so we can recover and continue!
|
269
|
250
|
* Calculate X at the next Y mesh line
|
270
|
251
|
*/
|
271
|
|
- const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
|
|
252
|
+ const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
|
272
|
253
|
|
273
|
|
- float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi)
|
|
254
|
+ float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
|
274
|
255
|
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
|
275
|
256
|
|
276
|
257
|
/**
|
|
@@ -282,7 +263,7 @@
|
282
|
263
|
*/
|
283
|
264
|
if (isnan(z0)) z0 = 0.0;
|
284
|
265
|
|
285
|
|
- const float y = mesh_index_to_ypos(current_yi);
|
|
266
|
+ const float ry = mesh_index_to_ypos(current_yi);
|
286
|
267
|
|
287
|
268
|
/**
|
288
|
269
|
* Without this check, it is possible for the algorithm to generate a zero length move in the case
|
|
@@ -290,9 +271,9 @@
|
290
|
271
|
* happens, it might be best to remove the check and always 'schedule' the move because
|
291
|
272
|
* the planner._buffer_line() routine will filter it if that happens.
|
292
|
273
|
*/
|
293
|
|
- if (y != start[Y_AXIS]) {
|
|
274
|
+ if (ry != start[Y_AXIS]) {
|
294
|
275
|
if (!inf_normalized_flag) {
|
295
|
|
- on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS];
|
|
276
|
+ on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
|
296
|
277
|
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
|
297
|
278
|
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
|
298
|
279
|
}
|
|
@@ -301,7 +282,7 @@
|
301
|
282
|
z_position = end[Z_AXIS];
|
302
|
283
|
}
|
303
|
284
|
|
304
|
|
- planner._buffer_line(x, y, z_position + z0, e_position, feed_rate, extruder);
|
|
285
|
+ planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
|
305
|
286
|
} //else printf("FIRST MOVE PRUNED ");
|
306
|
287
|
}
|
307
|
288
|
|
|
@@ -332,9 +313,9 @@
|
332
|
313
|
while (current_xi != cell_dest_xi + left_flag) {
|
333
|
314
|
current_xi += dxi;
|
334
|
315
|
const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
|
335
|
|
- y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
|
|
316
|
+ ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
|
336
|
317
|
|
337
|
|
- float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi)
|
|
318
|
+ float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
|
338
|
319
|
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
|
339
|
320
|
|
340
|
321
|
/**
|
|
@@ -346,7 +327,7 @@
|
346
|
327
|
*/
|
347
|
328
|
if (isnan(z0)) z0 = 0.0;
|
348
|
329
|
|
349
|
|
- const float x = mesh_index_to_xpos(current_xi);
|
|
330
|
+ const float rx = mesh_index_to_xpos(current_xi);
|
350
|
331
|
|
351
|
332
|
/**
|
352
|
333
|
* Without this check, it is possible for the algorithm to generate a zero length move in the case
|
|
@@ -354,9 +335,9 @@
|
354
|
335
|
* that happens, it might be best to remove the check and always 'schedule' the move because
|
355
|
336
|
* the planner._buffer_line() routine will filter it if that happens.
|
356
|
337
|
*/
|
357
|
|
- if (x != start[X_AXIS]) {
|
|
338
|
+ if (rx != start[X_AXIS]) {
|
358
|
339
|
if (!inf_normalized_flag) {
|
359
|
|
- on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS];
|
|
340
|
+ on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
|
360
|
341
|
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
|
361
|
342
|
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
|
362
|
343
|
}
|
|
@@ -365,7 +346,7 @@
|
365
|
346
|
z_position = end[Z_AXIS];
|
366
|
347
|
}
|
367
|
348
|
|
368
|
|
- planner._buffer_line(x, y, z_position + z0, e_position, feed_rate, extruder);
|
|
349
|
+ planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder);
|
369
|
350
|
} //else printf("FIRST MOVE PRUNED ");
|
370
|
351
|
}
|
371
|
352
|
|
|
@@ -398,15 +379,15 @@
|
398
|
379
|
|
399
|
380
|
const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
|
400
|
381
|
next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
|
401
|
|
- y = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
|
402
|
|
- x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
|
403
|
|
- // (No need to worry about m being zero.
|
404
|
|
- // If that was the case, it was already detected
|
405
|
|
- // as a vertical line move above.)
|
|
382
|
+ ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
|
|
383
|
+ rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
|
|
384
|
+ // (No need to worry about m being zero.
|
|
385
|
+ // If that was the case, it was already detected
|
|
386
|
+ // as a vertical line move above.)
|
406
|
387
|
|
407
|
|
- if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first
|
|
388
|
+ if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
|
408
|
389
|
// Yes! Crossing a Y Mesh Line next
|
409
|
|
- float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi)
|
|
390
|
+ float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
|
410
|
391
|
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
|
411
|
392
|
|
412
|
393
|
/**
|
|
@@ -419,7 +400,7 @@
|
419
|
400
|
if (isnan(z0)) z0 = 0.0;
|
420
|
401
|
|
421
|
402
|
if (!inf_normalized_flag) {
|
422
|
|
- on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
|
|
403
|
+ on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
|
423
|
404
|
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
|
424
|
405
|
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
|
425
|
406
|
}
|
|
@@ -427,13 +408,13 @@
|
427
|
408
|
e_position = end[E_AXIS];
|
428
|
409
|
z_position = end[Z_AXIS];
|
429
|
410
|
}
|
430
|
|
- planner._buffer_line(x, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
|
|
411
|
+ planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
|
431
|
412
|
current_yi += dyi;
|
432
|
413
|
yi_cnt--;
|
433
|
414
|
}
|
434
|
415
|
else {
|
435
|
416
|
// Yes! Crossing a X Mesh Line next
|
436
|
|
- float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag)
|
|
417
|
+ float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
|
437
|
418
|
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
|
438
|
419
|
|
439
|
420
|
/**
|
|
@@ -446,7 +427,7 @@
|
446
|
427
|
if (isnan(z0)) z0 = 0.0;
|
447
|
428
|
|
448
|
429
|
if (!inf_normalized_flag) {
|
449
|
|
- on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS];
|
|
430
|
+ on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
|
450
|
431
|
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
|
451
|
432
|
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
|
452
|
433
|
}
|
|
@@ -455,7 +436,7 @@
|
455
|
436
|
z_position = end[Z_AXIS];
|
456
|
437
|
}
|
457
|
438
|
|
458
|
|
- planner._buffer_line(next_mesh_line_x, y, z_position + z0, e_position, feed_rate, extruder);
|
|
439
|
+ planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
|
459
|
440
|
current_xi += dxi;
|
460
|
441
|
xi_cnt--;
|
461
|
442
|
}
|
|
@@ -489,29 +470,16 @@
|
489
|
470
|
// We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic,
|
490
|
471
|
// so we call _buffer_line directly here. Per-segmented leveling and kinematics performed first.
|
491
|
472
|
|
492
|
|
- inline void _O2 ubl_buffer_segment_raw(const float &rx, const float &ry, const float rz, const float &e, const float &fr) {
|
|
473
|
+ inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) {
|
493
|
474
|
|
494
|
475
|
#if ENABLED(DELTA) // apply delta inverse_kinematics
|
495
|
476
|
|
496
|
|
- const float delta_A = rz + SQRT( delta_diagonal_rod_2_tower[A_AXIS]
|
497
|
|
- - HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx,
|
498
|
|
- delta_tower[A_AXIS][Y_AXIS] - ry ));
|
499
|
|
-
|
500
|
|
- const float delta_B = rz + SQRT( delta_diagonal_rod_2_tower[B_AXIS]
|
501
|
|
- - HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx,
|
502
|
|
- delta_tower[B_AXIS][Y_AXIS] - ry ));
|
503
|
|
-
|
504
|
|
- const float delta_C = rz + SQRT( delta_diagonal_rod_2_tower[C_AXIS]
|
505
|
|
- - HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx,
|
506
|
|
- delta_tower[C_AXIS][Y_AXIS] - ry ));
|
|
477
|
+ DELTA_RAW_IK();
|
|
478
|
+ planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
|
507
|
479
|
|
508
|
|
- planner._buffer_line(delta_A, delta_B, delta_C, e, fr, active_extruder);
|
|
480
|
+ #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
|
509
|
481
|
|
510
|
|
- #elif IS_SCARA // apply scara inverse_kinematics
|
511
|
|
-
|
512
|
|
- const float lseg[XYZ] = { rx, ry, rz };
|
513
|
|
-
|
514
|
|
- inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ]
|
|
482
|
+ inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE]
|
515
|
483
|
// should move the feedrate scaling to scara inverse_kinematics
|
516
|
484
|
|
517
|
485
|
const float adiff = FABS(delta[A_AXIS] - scara_oldA),
|
|
@@ -520,14 +488,13 @@
|
520
|
488
|
scara_oldB = delta[B_AXIS];
|
521
|
489
|
float s_feedrate = max(adiff, bdiff) * scara_feed_factor;
|
522
|
490
|
|
523
|
|
- planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], e, s_feedrate, active_extruder);
|
|
491
|
+ planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder);
|
524
|
492
|
|
525
|
493
|
#else // CARTESIAN
|
526
|
494
|
|
527
|
|
- planner._buffer_line(rx, ry, rz, e, fr, active_extruder);
|
|
495
|
+ planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder);
|
528
|
496
|
|
529
|
497
|
#endif
|
530
|
|
-
|
531
|
498
|
}
|
532
|
499
|
|
533
|
500
|
|
|
@@ -542,12 +509,14 @@
|
542
|
509
|
if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary
|
543
|
510
|
return true; // did not move, so current_position still accurate
|
544
|
511
|
|
545
|
|
- const float tot_dx = rtarget[X_AXIS] - current_position[X_AXIS],
|
546
|
|
- tot_dy = rtarget[Y_AXIS] - current_position[Y_AXIS],
|
547
|
|
- tot_dz = rtarget[Z_AXIS] - current_position[Z_AXIS],
|
548
|
|
- tot_de = rtarget[E_AXIS] - current_position[E_AXIS];
|
|
512
|
+ const float total[XYZE] = {
|
|
513
|
+ rtarget[X_AXIS] - current_position[X_AXIS],
|
|
514
|
+ rtarget[Y_AXIS] - current_position[Y_AXIS],
|
|
515
|
+ rtarget[Z_AXIS] - current_position[Z_AXIS],
|
|
516
|
+ rtarget[E_AXIS] - current_position[E_AXIS]
|
|
517
|
+ };
|
549
|
518
|
|
550
|
|
- const float cartesian_xy_mm = HYPOT(tot_dx, tot_dy); // total horizontal xy distance
|
|
519
|
+ const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance
|
551
|
520
|
|
552
|
521
|
#if IS_KINEMATIC
|
553
|
522
|
const float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate
|
|
@@ -567,49 +536,30 @@
|
567
|
536
|
scara_oldB = stepper.get_axis_position_degrees(B_AXIS);
|
568
|
537
|
#endif
|
569
|
538
|
|
570
|
|
- const float seg_dx = tot_dx * inv_segments,
|
571
|
|
- seg_dy = tot_dy * inv_segments,
|
572
|
|
- seg_dz = tot_dz * inv_segments,
|
573
|
|
- seg_de = tot_de * inv_segments;
|
|
539
|
+ const float diff[XYZE] = {
|
|
540
|
+ total[X_AXIS] * inv_segments,
|
|
541
|
+ total[Y_AXIS] * inv_segments,
|
|
542
|
+ total[Z_AXIS] * inv_segments,
|
|
543
|
+ total[E_AXIS] * inv_segments
|
|
544
|
+ };
|
574
|
545
|
|
575
|
546
|
// Note that E segment distance could vary slightly as z mesh height
|
576
|
547
|
// changes for each segment, but small enough to ignore.
|
577
|
548
|
|
578
|
|
- float seg_rx = current_position[X_AXIS],
|
579
|
|
- seg_ry = current_position[Y_AXIS],
|
580
|
|
- seg_rz = current_position[Z_AXIS],
|
581
|
|
- seg_le = current_position[E_AXIS];
|
582
|
|
-
|
583
|
|
- const bool above_fade_height = (
|
584
|
|
- #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
585
|
|
- planner.z_fade_height != 0 && planner.z_fade_height < rtarget[Z_AXIS]
|
586
|
|
- #else
|
587
|
|
- false
|
588
|
|
- #endif
|
589
|
|
- );
|
|
549
|
+ float raw[XYZE] = {
|
|
550
|
+ current_position[X_AXIS],
|
|
551
|
+ current_position[Y_AXIS],
|
|
552
|
+ current_position[Z_AXIS],
|
|
553
|
+ current_position[E_AXIS]
|
|
554
|
+ };
|
590
|
555
|
|
591
|
556
|
// Only compute leveling per segment if ubl active and target below z_fade_height.
|
592
|
|
-
|
593
|
557
|
if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling
|
594
|
|
-
|
595
|
|
- do {
|
596
|
|
-
|
597
|
|
- if (--segments) { // not the last segment
|
598
|
|
- seg_rx += seg_dx;
|
599
|
|
- seg_ry += seg_dy;
|
600
|
|
- seg_rz += seg_dz;
|
601
|
|
- seg_le += seg_de;
|
602
|
|
- } else { // last segment, use exact destination
|
603
|
|
- seg_rx = rtarget[X_AXIS];
|
604
|
|
- seg_ry = rtarget[Y_AXIS];
|
605
|
|
- seg_rz = rtarget[Z_AXIS];
|
606
|
|
- seg_le = rtarget[E_AXIS];
|
607
|
|
- }
|
608
|
|
-
|
609
|
|
- ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz, seg_le, feedrate);
|
610
|
|
-
|
611
|
|
- } while (segments);
|
612
|
|
-
|
|
558
|
+ while (--segments) {
|
|
559
|
+ LOOP_XYZE(i) raw[i] += diff[i];
|
|
560
|
+ ubl_buffer_segment_raw(raw, feedrate);
|
|
561
|
+ }
|
|
562
|
+ ubl_buffer_segment_raw(rtarget, feedrate);
|
613
|
563
|
return false; // moved but did not set_current_from_destination();
|
614
|
564
|
}
|
615
|
565
|
|
|
@@ -617,15 +567,10 @@
|
617
|
567
|
|
618
|
568
|
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
619
|
569
|
const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]);
|
620
|
|
- #else
|
621
|
|
- constexpr float fade_scaling_factor = 1.0;
|
622
|
570
|
#endif
|
623
|
571
|
|
624
|
572
|
// increment to first segment destination
|
625
|
|
- seg_rx += seg_dx;
|
626
|
|
- seg_ry += seg_dy;
|
627
|
|
- seg_rz += seg_dz;
|
628
|
|
- seg_le += seg_de;
|
|
573
|
+ LOOP_XYZE(i) raw[i] += diff[i];
|
629
|
574
|
|
630
|
575
|
for(;;) { // for each mesh cell encountered during the move
|
631
|
576
|
|
|
@@ -636,8 +581,8 @@
|
636
|
581
|
// in top of loop and again re-find same adjacent cell and use it, just less efficient
|
637
|
582
|
// for mesh inset area.
|
638
|
583
|
|
639
|
|
- int8_t cell_xi = (seg_rx - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
|
640
|
|
- cell_yi = (seg_ry - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
|
|
584
|
+ int8_t cell_xi = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)),
|
|
585
|
+ cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST));
|
641
|
586
|
|
642
|
587
|
cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
|
643
|
588
|
cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1);
|
|
@@ -655,8 +600,8 @@
|
655
|
600
|
if (isnan(z_x0y1)) z_x0y1 = 0; // in order to avoid isnan tests per cell,
|
656
|
601
|
if (isnan(z_x1y1)) z_x1y1 = 0; // thus guessing zero for undefined points
|
657
|
602
|
|
658
|
|
- float cx = seg_rx - x0, // cell-relative x and y
|
659
|
|
- cy = seg_ry - y0;
|
|
603
|
+ float cx = raw[X_AXIS] - x0, // cell-relative x and y
|
|
604
|
+ cy = raw[Y_AXIS] - y0;
|
660
|
605
|
|
661
|
606
|
const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right)
|
662
|
607
|
z_xmy1 = (z_x1y1 - z_x0y1) * (1.0 / (MESH_X_DIST)); // z slope per x along y1 (upper left to upper right)
|
|
@@ -674,36 +619,35 @@
|
674
|
619
|
// and the z_cxym slope will change, both as a function of cx within the cell, and
|
675
|
620
|
// each change by a constant for fixed segment lengths.
|
676
|
621
|
|
677
|
|
- const float z_sxy0 = z_xmy0 * seg_dx, // per-segment adjustment to z_cxy0
|
678
|
|
- z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx; // per-segment adjustment to z_cxym
|
|
622
|
+ const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0
|
|
623
|
+ z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym
|
679
|
624
|
|
680
|
625
|
for(;;) { // for all segments within this mesh cell
|
681
|
626
|
|
682
|
|
- float z_cxcy = (z_cxy0 + z_cxym * cy) * fade_scaling_factor; // interpolated mesh z height along cx at cy, scaled for fade
|
|
627
|
+ if (--segments == 0) // if this is last segment, use rtarget for exact
|
|
628
|
+ COPY(raw, rtarget);
|
683
|
629
|
|
684
|
|
- if (--segments == 0) { // if this is last segment, use rtarget for exact
|
685
|
|
- seg_rx = rtarget[X_AXIS];
|
686
|
|
- seg_ry = rtarget[Y_AXIS];
|
687
|
|
- seg_rz = rtarget[Z_AXIS];
|
688
|
|
- seg_le = rtarget[E_AXIS];
|
689
|
|
- }
|
|
630
|
+ const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
|
|
631
|
+ #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
|
|
632
|
+ * fade_scaling_factor // apply fade factor to interpolated mesh height
|
|
633
|
+ #endif
|
|
634
|
+ ;
|
690
|
635
|
|
691
|
|
- ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate);
|
|
636
|
+ const float z = raw[Z_AXIS];
|
|
637
|
+ raw[Z_AXIS] += z_cxcy;
|
|
638
|
+ ubl_buffer_segment_raw(raw, feedrate);
|
|
639
|
+ raw[Z_AXIS] = z;
|
692
|
640
|
|
693
|
641
|
if (segments == 0) // done with last segment
|
694
|
642
|
return false; // did not set_current_from_destination()
|
695
|
643
|
|
696
|
|
- seg_rx += seg_dx;
|
697
|
|
- seg_ry += seg_dy;
|
698
|
|
- seg_rz += seg_dz;
|
699
|
|
- seg_le += seg_de;
|
|
644
|
+ LOOP_XYZE(i) raw[i] += diff[i];
|
700
|
645
|
|
701
|
|
- cx += seg_dx;
|
702
|
|
- cy += seg_dy;
|
|
646
|
+ cx += diff[X_AXIS];
|
|
647
|
+ cy += diff[Y_AXIS];
|
703
|
648
|
|
704
|
|
- if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) { // done within this cell, break to next
|
|
649
|
+ if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next
|
705
|
650
|
break;
|
706
|
|
- }
|
707
|
651
|
|
708
|
652
|
// Next segment still within same mesh cell, adjust the per-segment
|
709
|
653
|
// slope and intercept to compute next z height.
|
|
@@ -718,4 +662,3 @@
|
718
|
662
|
#endif // UBL_DELTA
|
719
|
663
|
|
720
|
664
|
#endif // AUTO_BED_LEVELING_UBL
|
721
|
|
-
|