|
@@ -9572,77 +9572,17 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) {
|
9572
|
9572
|
float logical[XYZE];
|
9573
|
9573
|
COPY(logical, current_position);
|
9574
|
9574
|
|
9575
|
|
- #if ENABLED(USE_DELTA_IK_INTERPOLATION)
|
9576
|
|
-
|
9577
|
|
- // Only interpolate XYZ. Advance E normally.
|
9578
|
|
- #define DELTA_NEXT(ADDEND) LOOP_XYZ(i) logical[i] += ADDEND;
|
9579
|
|
-
|
9580
|
|
- // Get the starting delta if interpolation is possible
|
9581
|
|
- if (segments >= 2) {
|
9582
|
|
- DELTA_IK();
|
9583
|
|
- ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
|
9584
|
|
- }
|
9585
|
|
-
|
9586
|
|
- // Loop using decrement
|
9587
|
|
- for (uint16_t s = segments + 1; --s;) {
|
9588
|
|
- // Are there at least 2 moves left?
|
9589
|
|
- if (s >= 2) {
|
9590
|
|
- // Save the previous delta for interpolation
|
9591
|
|
- float prev_delta[ABC] = { delta[A_AXIS], delta[B_AXIS], delta[C_AXIS] };
|
9592
|
|
-
|
9593
|
|
- // Get the delta 2 segments ahead (rather than the next)
|
9594
|
|
- DELTA_NEXT(segment_distance[i] + segment_distance[i]);
|
9595
|
|
-
|
9596
|
|
- // Advance E normally
|
9597
|
|
- logical[E_AXIS] += segment_distance[E_AXIS];
|
9598
|
|
-
|
9599
|
|
- // Get the exact delta for the move after this
|
9600
|
|
- DELTA_IK();
|
9601
|
|
- ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
|
9602
|
|
-
|
9603
|
|
- // Move to the interpolated delta position first
|
9604
|
|
- planner.buffer_line(
|
9605
|
|
- (prev_delta[A_AXIS] + delta[A_AXIS]) * 0.5,
|
9606
|
|
- (prev_delta[B_AXIS] + delta[B_AXIS]) * 0.5,
|
9607
|
|
- (prev_delta[C_AXIS] + delta[C_AXIS]) * 0.5,
|
9608
|
|
- logical[E_AXIS], _feedrate_mm_s, active_extruder
|
9609
|
|
- );
|
9610
|
|
-
|
9611
|
|
- // Advance E once more for the next move
|
9612
|
|
- logical[E_AXIS] += segment_distance[E_AXIS];
|
9613
|
|
-
|
9614
|
|
- // Do an extra decrement of the loop
|
9615
|
|
- --s;
|
9616
|
|
- }
|
9617
|
|
- else {
|
9618
|
|
- // Get the last segment delta. (Used when segments is odd)
|
9619
|
|
- DELTA_NEXT(segment_distance[i]);
|
9620
|
|
- logical[E_AXIS] += segment_distance[E_AXIS];
|
9621
|
|
- DELTA_IK();
|
9622
|
|
- ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
|
9623
|
|
- }
|
9624
|
|
-
|
9625
|
|
- // Move to the non-interpolated position
|
9626
|
|
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
9627
|
|
- }
|
9628
|
|
-
|
9629
|
|
- #else
|
9630
|
|
-
|
9631
|
|
- #define DELTA_NEXT(ADDEND) LOOP_XYZE(i) logical[i] += ADDEND;
|
9632
|
|
-
|
9633
|
|
- // For non-interpolated delta calculate every segment
|
9634
|
|
- for (uint16_t s = segments + 1; --s;) {
|
9635
|
|
- DELTA_NEXT(segment_distance[i]);
|
9636
|
|
- #if ENABLED(DELTA)
|
9637
|
|
- DELTA_LOGICAL_IK(); // Delta can inline its kinematics
|
9638
|
|
- #else
|
9639
|
|
- inverse_kinematics(logical);
|
9640
|
|
- #endif
|
9641
|
|
- ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
|
9642
|
|
- planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
9643
|
|
- }
|
9644
|
|
-
|
9645
|
|
- #endif
|
|
9575
|
+ // Calculate and execute the segments
|
|
9576
|
+ for (uint16_t s = segments + 1; --s;) {
|
|
9577
|
+ LOOP_XYZE(i) logical[i] += segment_distance[i];
|
|
9578
|
+ #if ENABLED(DELTA)
|
|
9579
|
+ DELTA_LOGICAL_IK(); // Delta can inline its kinematics
|
|
9580
|
+ #else
|
|
9581
|
+ inverse_kinematics(logical);
|
|
9582
|
+ #endif
|
|
9583
|
+ ADJUST_DELTA(logical); // Adjust Z if bed leveling is enabled
|
|
9584
|
+ planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], _feedrate_mm_s, active_extruder);
|
|
9585
|
+ }
|
9646
|
9586
|
|
9647
|
9587
|
// Since segment_distance is only approximate,
|
9648
|
9588
|
// the final move must be to the exact destination.
|