|
@@ -100,13 +100,18 @@ Planner planner;
|
100
|
100
|
* A ring buffer of moves described in steps
|
101
|
101
|
*/
|
102
|
102
|
block_t Planner::block_buffer[BLOCK_BUFFER_SIZE];
|
103
|
|
-volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
|
104
|
|
- Planner::block_buffer_tail;
|
|
103
|
+volatile uint8_t Planner::block_buffer_head, // Index of the next block to be pushed
|
|
104
|
+ Planner::block_buffer_tail; // Index of the busy block, if any
|
|
105
|
+uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks
|
105
|
106
|
|
106
|
|
-float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
|
|
107
|
+float Planner::max_feedrate_mm_s[XYZE_N], // Max speeds in mm per second
|
107
|
108
|
Planner::axis_steps_per_mm[XYZE_N],
|
108
|
109
|
Planner::steps_to_mm[XYZE_N];
|
109
|
110
|
|
|
111
|
+#if ENABLED(ABORT_ON_ENDSTOP_HIT_FEATURE_ENABLED)
|
|
112
|
+ bool Planner::abort_on_endstop_hit = false;
|
|
113
|
+#endif
|
|
114
|
+
|
110
|
115
|
#if ENABLED(DISTINCT_E_FACTORS)
|
111
|
116
|
uint8_t Planner::last_extruder = 0; // Respond to extruder change
|
112
|
117
|
#endif
|
|
@@ -175,7 +180,7 @@ int32_t Planner::position[NUM_AXIS] = { 0 };
|
175
|
180
|
uint32_t Planner::cutoff_long;
|
176
|
181
|
|
177
|
182
|
float Planner::previous_speed[NUM_AXIS],
|
178
|
|
- Planner::previous_nominal_speed;
|
|
183
|
+ Planner::previous_nominal_speed_sqr;
|
179
|
184
|
|
180
|
185
|
#if ENABLED(DISABLE_INACTIVE_EXTRUDER)
|
181
|
186
|
uint8_t Planner::g_uc_extruder_last_move[EXTRUDERS] = { 0 };
|
|
@@ -212,7 +217,7 @@ void Planner::init() {
|
212
|
217
|
ZERO(position_float);
|
213
|
218
|
#endif
|
214
|
219
|
ZERO(previous_speed);
|
215
|
|
- previous_nominal_speed = 0.0;
|
|
220
|
+ previous_nominal_speed_sqr = 0.0;
|
216
|
221
|
#if ABL_PLANAR
|
217
|
222
|
bed_level_matrix.set_to_identity();
|
218
|
223
|
#endif
|
|
@@ -363,7 +368,7 @@ void Planner::init() {
|
363
|
368
|
//
|
364
|
369
|
static uint32_t get_period_inverse(uint32_t d) {
|
365
|
370
|
|
366
|
|
- static const uint8_t inv_tab[256] PROGMEM = {
|
|
371
|
+ static const uint8_t inv_tab[256] PROGMEM = {
|
367
|
372
|
255,253,252,250,248,246,244,242,240,238,236,234,233,231,229,227,
|
368
|
373
|
225,224,222,220,218,217,215,213,212,210,208,207,205,203,202,200,
|
369
|
374
|
199,197,195,194,192,191,189,188,186,185,183,182,180,179,178,176,
|
|
@@ -727,12 +732,9 @@ void Planner::init() {
|
727
|
732
|
}
|
728
|
733
|
#else
|
729
|
734
|
// All the other 32 CPUs can easily perform the inverse using hardware division,
|
730
|
|
- // so we don´t need to reduce precision or to use assembly language at all.
|
731
|
|
-
|
|
735
|
+ // so we don't need to reduce precision or to use assembly language at all.
|
732
|
736
|
// This routine, for all the other archs, returns 0x100000000 / d ~= 0xFFFFFFFF / d
|
733
|
|
- static FORCE_INLINE uint32_t get_period_inverse(uint32_t d) {
|
734
|
|
- return 0xFFFFFFFF / d;
|
735
|
|
- }
|
|
737
|
+ static FORCE_INLINE uint32_t get_period_inverse(const uint32_t d) { return 0xFFFFFFFF / d; }
|
736
|
738
|
#endif
|
737
|
739
|
#endif
|
738
|
740
|
|
|
@@ -743,6 +745,7 @@ void Planner::init() {
|
743
|
745
|
* by the provided factors.
|
744
|
746
|
*/
|
745
|
747
|
void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
|
|
748
|
+
|
746
|
749
|
uint32_t initial_rate = CEIL(block->nominal_rate * entry_factor),
|
747
|
750
|
final_rate = CEIL(block->nominal_rate * exit_factor); // (steps per second)
|
748
|
751
|
|
|
@@ -757,19 +760,18 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
|
757
|
760
|
const int32_t accel = block->acceleration_steps_per_s2;
|
758
|
761
|
|
759
|
762
|
// Steps required for acceleration, deceleration to/from nominal rate
|
760
|
|
- int32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
|
761
|
|
- decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel)),
|
|
763
|
+ uint32_t accelerate_steps = CEIL(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
|
|
764
|
+ decelerate_steps = FLOOR(estimate_acceleration_distance(block->nominal_rate, final_rate, -accel));
|
762
|
765
|
// Steps between acceleration and deceleration, if any
|
763
|
|
- plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
|
|
766
|
+ int32_t plateau_steps = block->step_event_count - accelerate_steps - decelerate_steps;
|
764
|
767
|
|
765
|
768
|
// Does accelerate_steps + decelerate_steps exceed step_event_count?
|
766
|
769
|
// Then we can't possibly reach the nominal rate, there will be no cruising.
|
767
|
770
|
// Use intersection_distance() to calculate accel / braking time in order to
|
768
|
771
|
// reach the final_rate exactly at the end of this block.
|
769
|
772
|
if (plateau_steps < 0) {
|
770
|
|
- accelerate_steps = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
|
771
|
|
- NOLESS(accelerate_steps, 0); // Check limits due to numerical round-off
|
772
|
|
- accelerate_steps = min((uint32_t)accelerate_steps, block->step_event_count);//(We can cast here to unsigned, because the above line ensures that we are above zero)
|
|
773
|
+ const float accelerate_steps_float = CEIL(intersection_distance(initial_rate, final_rate, accel, block->step_event_count));
|
|
774
|
+ accelerate_steps = MIN(uint32_t(MAX(accelerate_steps_float, 0)), block->step_event_count);
|
773
|
775
|
plateau_steps = 0;
|
774
|
776
|
|
775
|
777
|
#if ENABLED(BEZIER_JERK_CONTROL)
|
|
@@ -796,7 +798,10 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
|
796
|
798
|
|
797
|
799
|
#endif
|
798
|
800
|
|
799
|
|
- CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
|
|
801
|
+ // Fill variables used by the stepper in a critical section
|
|
802
|
+ const bool was_enabled = STEPPER_ISR_ENABLED();
|
|
803
|
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
|
804
|
+
|
800
|
805
|
if (!TEST(block->flag, BLOCK_BIT_BUSY)) { // Don't update variables if block is busy.
|
801
|
806
|
block->accelerate_until = accelerate_steps;
|
802
|
807
|
block->decelerate_after = accelerate_steps + plateau_steps;
|
|
@@ -810,32 +815,35 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
|
810
|
815
|
#endif
|
811
|
816
|
block->final_rate = final_rate;
|
812
|
817
|
}
|
813
|
|
- CRITICAL_SECTION_END;
|
|
818
|
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
814
|
819
|
}
|
815
|
820
|
|
816
|
|
-// "Junction jerk" in this context is the immediate change in speed at the junction of two blocks.
|
817
|
|
-// This method will calculate the junction jerk as the euclidean distance between the nominal
|
818
|
|
-// velocities of the respective blocks.
|
819
|
|
-//inline float junction_jerk(block_t *before, block_t *after) {
|
820
|
|
-// return SQRT(
|
821
|
|
-// POW((before->speed_x-after->speed_x), 2)+POW((before->speed_y-after->speed_y), 2));
|
822
|
|
-//}
|
823
|
|
-
|
824
|
821
|
// The kernel called by recalculate() when scanning the plan from last to first entry.
|
825
|
|
-void Planner::reverse_pass_kernel(block_t* const current, const block_t* const next) {
|
826
|
|
- if (current && next) {
|
827
|
|
- // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
|
828
|
|
- // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
|
829
|
|
- // check for maximum allowable speed reductions to ensure maximum possible planned speed.
|
830
|
|
- const float max_entry_speed = current->max_entry_speed;
|
831
|
|
- if (current->entry_speed != max_entry_speed || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
|
832
|
|
- // If nominal length true, max junction speed is guaranteed to be reached. Only compute
|
833
|
|
- // for max allowable speed if block is decelerating and nominal length is false.
|
834
|
|
- const float new_entry_speed = (TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
|
835
|
|
- ? max_entry_speed
|
836
|
|
- : MIN(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
|
837
|
|
- if (new_entry_speed != current->entry_speed) {
|
838
|
|
- current->entry_speed = new_entry_speed;
|
|
822
|
+void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
|
|
823
|
+ if (current) {
|
|
824
|
+ // If entry speed is already at the maximum entry speed, and there was no change of speed
|
|
825
|
+ // in the next block, there is no need to recheck. Block is cruising and there is no need to
|
|
826
|
+ // compute anything for this block,
|
|
827
|
+ // If not, block entry speed needs to be recalculated to ensure maximum possible planned speed.
|
|
828
|
+ const float max_entry_speed_sqr = current->max_entry_speed_sqr;
|
|
829
|
+
|
|
830
|
+ // Compute maximum entry speed decelerating over the current block from its exit speed.
|
|
831
|
+ // If not at the maximum entry speed, or the previous block entry speed changed
|
|
832
|
+ if (current->entry_speed_sqr != max_entry_speed_sqr || (next && TEST(next->flag, BLOCK_BIT_RECALCULATE))) {
|
|
833
|
+
|
|
834
|
+ // If nominal length true, max junction speed is guaranteed to be reached.
|
|
835
|
+ // If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
|
|
836
|
+ // the current block and next block junction speeds are guaranteed to always be at their maximum
|
|
837
|
+ // junction speeds in deceleration and acceleration, respectively. This is due to how the current
|
|
838
|
+ // block nominal speed limits both the current and next maximum junction speeds. Hence, in both
|
|
839
|
+ // the reverse and forward planners, the corresponding block junction speed will always be at the
|
|
840
|
+ // the maximum junction speed and may always be ignored for any speed reduction checks.
|
|
841
|
+
|
|
842
|
+ const float new_entry_speed_sqr = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
|
|
843
|
+ ? max_entry_speed_sqr
|
|
844
|
+ : MIN(max_entry_speed_sqr, max_allowable_speed_sqr(-current->acceleration, next ? next->entry_speed_sqr : sq(MINIMUM_PLANNER_SPEED), current->millimeters));
|
|
845
|
+ if (current->entry_speed_sqr != new_entry_speed_sqr) {
|
|
846
|
+ current->entry_speed_sqr = new_entry_speed_sqr;
|
839
|
847
|
SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
840
|
848
|
}
|
841
|
849
|
}
|
|
@@ -850,44 +858,37 @@ void Planner::reverse_pass() {
|
850
|
858
|
if (movesplanned() > 2) {
|
851
|
859
|
const uint8_t endnr = next_block_index(block_buffer_tail); // tail is running. tail+1 shouldn't be altered because it's connected to the running block.
|
852
|
860
|
uint8_t blocknr = prev_block_index(block_buffer_head);
|
853
|
|
- block_t* current = &block_buffer[blocknr];
|
854
|
|
-
|
855
|
|
- // Last/newest block in buffer:
|
856
|
|
- const float max_entry_speed = current->max_entry_speed;
|
857
|
|
- if (current->entry_speed != max_entry_speed) {
|
858
|
|
- // If nominal length true, max junction speed is guaranteed to be reached. Only compute
|
859
|
|
- // for max allowable speed if block is decelerating and nominal length is false.
|
860
|
|
- const float new_entry_speed = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
|
861
|
|
- ? max_entry_speed
|
862
|
|
- : MIN(max_entry_speed, max_allowable_speed(-current->acceleration, MINIMUM_PLANNER_SPEED, current->millimeters));
|
863
|
|
- if (current->entry_speed != new_entry_speed) {
|
864
|
|
- current->entry_speed = new_entry_speed;
|
865
|
|
- SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
866
|
|
- }
|
867
|
|
- }
|
868
|
861
|
|
869
|
|
- do {
|
870
|
|
- const block_t * const next = current;
|
871
|
|
- blocknr = prev_block_index(blocknr);
|
|
862
|
+ // Perform the reverse pass
|
|
863
|
+ block_t *current, *next = NULL;
|
|
864
|
+ while (blocknr != endnr) {
|
|
865
|
+ // Perform the reverse pass - Only consider non sync blocks
|
872
|
866
|
current = &block_buffer[blocknr];
|
873
|
|
- reverse_pass_kernel(current, next);
|
874
|
|
- } while (blocknr != endnr);
|
|
867
|
+ if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
|
|
868
|
+ reverse_pass_kernel(current, next);
|
|
869
|
+ next = current;
|
|
870
|
+ }
|
|
871
|
+ // Advance to the next
|
|
872
|
+ blocknr = prev_block_index(blocknr);
|
|
873
|
+ }
|
875
|
874
|
}
|
876
|
875
|
}
|
877
|
876
|
|
878
|
877
|
// The kernel called by recalculate() when scanning the plan from first to last entry.
|
879
|
|
-void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current) {
|
|
878
|
+void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current) {
|
880
|
879
|
if (previous) {
|
881
|
880
|
// If the previous block is an acceleration block, too short to complete the full speed
|
882
|
881
|
// change, adjust the entry speed accordingly. Entry speeds have already been reset,
|
883
|
882
|
// maximized, and reverse-planned. If nominal length is set, max junction speed is
|
884
|
883
|
// guaranteed to be reached. No need to recheck.
|
885
|
884
|
if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) {
|
886
|
|
- if (previous->entry_speed < current->entry_speed) {
|
887
|
|
- const float new_entry_speed = MIN(current->entry_speed, max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
|
888
|
|
- // Check for junction speed change
|
889
|
|
- if (current->entry_speed != new_entry_speed) {
|
890
|
|
- current->entry_speed = new_entry_speed;
|
|
885
|
+ if (previous->entry_speed_sqr < current->entry_speed_sqr) {
|
|
886
|
+ // Compute the maximum allowable speed
|
|
887
|
+ const float new_entry_speed_sqr = max_allowable_speed_sqr(-previous->acceleration, previous->entry_speed_sqr, previous->millimeters);
|
|
888
|
+ // If true, current block is full-acceleration
|
|
889
|
+ if (current->entry_speed_sqr > new_entry_speed_sqr) {
|
|
890
|
+ // Always <= max_entry_speed_sqr. Backward pass sets this.
|
|
891
|
+ current->entry_speed_sqr = new_entry_speed_sqr;
|
891
|
892
|
SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
892
|
893
|
}
|
893
|
894
|
}
|
|
@@ -900,15 +901,21 @@ void Planner::forward_pass_kernel(const block_t* const previous, block_t* const
|
900
|
901
|
* Once in reverse and once forward. This implements the forward pass.
|
901
|
902
|
*/
|
902
|
903
|
void Planner::forward_pass() {
|
903
|
|
- block_t* block[3] = { NULL, NULL, NULL };
|
904
|
|
-
|
905
|
|
- for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
|
906
|
|
- block[0] = block[1];
|
907
|
|
- block[1] = block[2];
|
908
|
|
- block[2] = &block_buffer[b];
|
909
|
|
- forward_pass_kernel(block[0], block[1]);
|
|
904
|
+ const uint8_t endnr = block_buffer_head;
|
|
905
|
+ uint8_t blocknr = block_buffer_tail;
|
|
906
|
+
|
|
907
|
+ // Perform the forward pass
|
|
908
|
+ block_t *current, *previous = NULL;
|
|
909
|
+ while (blocknr != endnr) {
|
|
910
|
+ // Perform the forward pass - Only consider non-sync blocks
|
|
911
|
+ current = &block_buffer[blocknr];
|
|
912
|
+ if (!TEST(current->flag, BLOCK_BIT_SYNC_POSITION)) {
|
|
913
|
+ forward_pass_kernel(previous, current);
|
|
914
|
+ previous = current;
|
|
915
|
+ }
|
|
916
|
+ // Advance to the previous
|
|
917
|
+ blocknr = next_block_index(blocknr);
|
910
|
918
|
}
|
911
|
|
- forward_pass_kernel(block[1], block[2]);
|
912
|
919
|
}
|
913
|
920
|
|
914
|
921
|
/**
|
|
@@ -917,38 +924,72 @@ void Planner::forward_pass() {
|
917
|
924
|
* recalculate() after updating the blocks.
|
918
|
925
|
*/
|
919
|
926
|
void Planner::recalculate_trapezoids() {
|
920
|
|
- int8_t block_index = block_buffer_tail;
|
921
|
|
- block_t *current, *next = NULL;
|
|
927
|
+ uint8_t block_index = block_buffer_tail;
|
|
928
|
+
|
|
929
|
+ // As there could be a sync block in the head of the queue, and the next loop must not
|
|
930
|
+ // recalculate the head block (as it needs to be specially handled), scan backwards until
|
|
931
|
+ // we find the first non SYNC block
|
|
932
|
+ uint8_t head_block_index = block_buffer_head;
|
|
933
|
+ while (head_block_index != block_index) {
|
|
934
|
+
|
|
935
|
+ // Go back (head always point to the first free block)
|
|
936
|
+ uint8_t prev_index = prev_block_index(head_block_index);
|
|
937
|
+
|
|
938
|
+ // Get the pointer to the block
|
|
939
|
+ block_t *prev = &block_buffer[prev_index];
|
|
940
|
+
|
|
941
|
+ // If not dealing with a sync block, we are done. The last block is not a SYNC block
|
|
942
|
+ if (!TEST(prev->flag, BLOCK_BIT_SYNC_POSITION)) break;
|
|
943
|
+
|
|
944
|
+ // Examine the previous block. This and all following are SYNC blocks
|
|
945
|
+ head_block_index = prev_index;
|
|
946
|
+ };
|
|
947
|
+
|
|
948
|
+ // Go from the tail (currently executed block) to the first block, without including it)
|
|
949
|
+ block_t *current = NULL, *next = NULL;
|
|
950
|
+ float current_entry_speed = 0.0, next_entry_speed = 0.0;
|
|
951
|
+ while (block_index != head_block_index) {
|
922
|
952
|
|
923
|
|
- while (block_index != block_buffer_head) {
|
924
|
|
- current = next;
|
925
|
953
|
next = &block_buffer[block_index];
|
926
|
|
- if (current) {
|
927
|
|
- // Recalculate if current block entry or exit junction speed has changed.
|
928
|
|
- if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
|
929
|
|
- // NOTE: Entry and exit factors always > 0 by all previous logic operations.
|
930
|
|
- const float nomr = 1.0 / current->nominal_speed;
|
931
|
|
- calculate_trapezoid_for_block(current, current->entry_speed * nomr, next->entry_speed * nomr);
|
932
|
|
- #if ENABLED(LIN_ADVANCE)
|
933
|
|
- if (current->use_advance_lead) {
|
934
|
|
- const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
|
935
|
|
- current->max_adv_steps = current->nominal_speed * comp;
|
936
|
|
- current->final_adv_steps = next->entry_speed * comp;
|
937
|
|
- }
|
938
|
|
- #endif
|
939
|
|
- CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
|
|
954
|
+
|
|
955
|
+ // Skip sync blocks
|
|
956
|
+ if (!TEST(next->flag, BLOCK_BIT_SYNC_POSITION)) {
|
|
957
|
+ next_entry_speed = SQRT(next->entry_speed_sqr);
|
|
958
|
+
|
|
959
|
+ if (current) {
|
|
960
|
+ // Recalculate if current block entry or exit junction speed has changed.
|
|
961
|
+ if (TEST(current->flag, BLOCK_BIT_RECALCULATE) || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
|
|
962
|
+ // NOTE: Entry and exit factors always > 0 by all previous logic operations.
|
|
963
|
+ const float current_nominal_speed = SQRT(current->nominal_speed_sqr),
|
|
964
|
+ nomr = 1.0 / current_nominal_speed;
|
|
965
|
+ calculate_trapezoid_for_block(current, current_entry_speed * nomr, next_entry_speed * nomr);
|
|
966
|
+ #if ENABLED(LIN_ADVANCE)
|
|
967
|
+ if (current->use_advance_lead) {
|
|
968
|
+ const float comp = current->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
|
|
969
|
+ current->max_adv_steps = current_nominal_speed * comp;
|
|
970
|
+ current->final_adv_steps = next_entry_speed * comp;
|
|
971
|
+ }
|
|
972
|
+ #endif
|
|
973
|
+ CBI(current->flag, BLOCK_BIT_RECALCULATE); // Reset current only to ensure next trapezoid is computed
|
|
974
|
+ }
|
940
|
975
|
}
|
|
976
|
+
|
|
977
|
+ current = next;
|
|
978
|
+ current_entry_speed = next_entry_speed;
|
941
|
979
|
}
|
|
980
|
+
|
942
|
981
|
block_index = next_block_index(block_index);
|
943
|
982
|
}
|
|
983
|
+
|
944
|
984
|
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
|
945
|
985
|
if (next) {
|
946
|
|
- const float nomr = 1.0 / next->nominal_speed;
|
947
|
|
- calculate_trapezoid_for_block(next, next->entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
|
|
986
|
+ const float next_nominal_speed = SQRT(next->nominal_speed_sqr),
|
|
987
|
+ nomr = 1.0 / next_nominal_speed;
|
|
988
|
+ calculate_trapezoid_for_block(next, next_entry_speed * nomr, (MINIMUM_PLANNER_SPEED) * nomr);
|
948
|
989
|
#if ENABLED(LIN_ADVANCE)
|
949
|
990
|
if (next->use_advance_lead) {
|
950
|
991
|
const float comp = next->e_D_ratio * extruder_advance_K * axis_steps_per_mm[E_AXIS];
|
951
|
|
- next->max_adv_steps = next->nominal_speed * comp;
|
|
992
|
+ next->max_adv_steps = next_nominal_speed * comp;
|
952
|
993
|
next->final_adv_steps = (MINIMUM_PLANNER_SPEED) * comp;
|
953
|
994
|
}
|
954
|
995
|
#endif
|
|
@@ -998,7 +1039,7 @@ void Planner::recalculate() {
|
998
|
1039
|
for (uint8_t b = block_buffer_tail; b != block_buffer_head; b = next_block_index(b)) {
|
999
|
1040
|
block_t* block = &block_buffer[b];
|
1000
|
1041
|
if (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS]) {
|
1001
|
|
- float se = (float)block->steps[E_AXIS] / block->step_event_count * block->nominal_speed; // mm/sec;
|
|
1042
|
+ const float se = (float)block->steps[E_AXIS] / block->step_event_count * SQRT(block->nominal_speed_sqr); // mm/sec;
|
1002
|
1043
|
NOLESS(high, se);
|
1003
|
1044
|
}
|
1004
|
1045
|
}
|
|
@@ -1299,6 +1340,59 @@ void Planner::check_axes_activity() {
|
1299
|
1340
|
|
1300
|
1341
|
#endif // PLANNER_LEVELING
|
1301
|
1342
|
|
|
1343
|
+void Planner::quick_stop() {
|
|
1344
|
+ // Remove all the queued blocks. Note that this function is NOT
|
|
1345
|
+ // called from the Stepper ISR, so we must consider tail as readonly!
|
|
1346
|
+ // that is why we set head to tail!
|
|
1347
|
+ block_buffer_head = block_buffer_tail;
|
|
1348
|
+
|
|
1349
|
+ #if ENABLED(ULTRA_LCD)
|
|
1350
|
+ // Clear the accumulated runtime
|
|
1351
|
+ clear_block_buffer_runtime();
|
|
1352
|
+ #endif
|
|
1353
|
+
|
|
1354
|
+ // Make sure to drop any attempt of queuing moves for at least 1 second
|
|
1355
|
+ cleaning_buffer_counter = 1000;
|
|
1356
|
+
|
|
1357
|
+ // And stop the stepper ISR
|
|
1358
|
+ stepper.quick_stop();
|
|
1359
|
+}
|
|
1360
|
+
|
|
1361
|
+void Planner::endstop_triggered(const AxisEnum axis) {
|
|
1362
|
+
|
|
1363
|
+ /*NB: This will be called via endstops.update()
|
|
1364
|
+ and endstops.update() can be called from the temperature
|
|
1365
|
+ ISR. So Stepper interrupts are enabled */
|
|
1366
|
+
|
|
1367
|
+ // Disable stepper ISR
|
|
1368
|
+ bool stepper_isr_enabled = STEPPER_ISR_ENABLED();
|
|
1369
|
+ DISABLE_STEPPER_DRIVER_INTERRUPT();
|
|
1370
|
+
|
|
1371
|
+ // Record stepper position
|
|
1372
|
+ stepper.endstop_triggered(axis);
|
|
1373
|
+
|
|
1374
|
+ // Discard the active block that led to the trigger
|
|
1375
|
+ discard_current_block();
|
|
1376
|
+
|
|
1377
|
+ // Discard the CONTINUED block, if any. Note the planner can only queue 1 continued
|
|
1378
|
+ // block after a previous non continued block, as the condition to queue them
|
|
1379
|
+ // is that there are no queued blocks at the time a new block is queued.
|
|
1380
|
+ const bool discard = has_blocks_queued() && TEST(block_buffer[block_buffer_tail].flag, BLOCK_BIT_CONTINUED);
|
|
1381
|
+ if (discard) discard_current_block();
|
|
1382
|
+
|
|
1383
|
+ // Reenable stepper ISR if it was enabled
|
|
1384
|
+ if (stepper_isr_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
|
1385
|
+}
|
|
1386
|
+
|
|
1387
|
+float Planner::triggered_position_mm(const AxisEnum axis) {
|
|
1388
|
+ return stepper.triggered_position(axis) * steps_to_mm[axis];
|
|
1389
|
+}
|
|
1390
|
+
|
|
1391
|
+void Planner::finish_and_disable() {
|
|
1392
|
+ while (has_blocks_queued() || cleaning_buffer_counter) idle();
|
|
1393
|
+ disable_all_steppers();
|
|
1394
|
+}
|
|
1395
|
+
|
1302
|
1396
|
/**
|
1303
|
1397
|
* Get an axis position according to stepper position(s)
|
1304
|
1398
|
* For CORE machines apply translation from ABC to XYZ.
|
|
@@ -1311,7 +1405,7 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
|
1311
|
1405
|
|
1312
|
1406
|
// Protect the access to the position.
|
1313
|
1407
|
const bool was_enabled = STEPPER_ISR_ENABLED();
|
1314
|
|
- DISABLE_STEPPER_DRIVER_INTERRUPT();
|
|
1408
|
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
1315
|
1409
|
|
1316
|
1410
|
// ((a1+a2)+(a1-a2))/2 -> (a1+a2+a1-a2)/2 -> (a1+a1)/2 -> a1
|
1317
|
1411
|
// ((a1+a2)-(a1-a2))/2 -> (a1+a2-a1+a2)/2 -> (a2+a2)/2 -> a2
|
|
@@ -1333,18 +1427,69 @@ float Planner::get_axis_position_mm(const AxisEnum axis) {
|
1333
|
1427
|
/**
|
1334
|
1428
|
* Block until all buffered steps are executed / cleaned
|
1335
|
1429
|
*/
|
1336
|
|
-void Planner::synchronize() { while (has_blocks_queued() || stepper.cleaning_buffer_counter) idle(); }
|
|
1430
|
+void Planner::synchronize() { while (has_blocks_queued() || cleaning_buffer_counter) idle(); }
|
1337
|
1431
|
|
1338
|
1432
|
/**
|
1339
|
1433
|
* Planner::_buffer_steps
|
1340
|
1434
|
*
|
1341
|
|
- * Add a new linear movement to the buffer (in terms of steps).
|
|
1435
|
+ * Add a new linear movement to the planner queue (in terms of steps).
|
|
1436
|
+ *
|
|
1437
|
+ * target - target position in steps units
|
|
1438
|
+ * fr_mm_s - (target) speed of the move
|
|
1439
|
+ * extruder - target extruder
|
|
1440
|
+ * millimeters - the length of the movement, if known
|
|
1441
|
+ *
|
|
1442
|
+ * Returns true if movement was properly queued, false otherwise
|
|
1443
|
+ */
|
|
1444
|
+bool Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
|
1445
|
+ #if HAS_POSITION_FLOAT
|
|
1446
|
+ , const float (&target_float)[XYZE]
|
|
1447
|
+ #endif
|
|
1448
|
+ , float fr_mm_s, const uint8_t extruder, const float &millimeters
|
|
1449
|
+) {
|
|
1450
|
+
|
|
1451
|
+ // If we are cleaning, do not accept queuing of movements
|
|
1452
|
+ if (cleaning_buffer_counter) return false;
|
|
1453
|
+
|
|
1454
|
+ // Wait for the next available block
|
|
1455
|
+ uint8_t next_buffer_head;
|
|
1456
|
+ block_t * const block = get_next_free_block(next_buffer_head);
|
|
1457
|
+
|
|
1458
|
+ // Fill the block with the specified movement
|
|
1459
|
+ if (!_populate_block(block, false, target
|
|
1460
|
+ #if HAS_POSITION_FLOAT
|
|
1461
|
+ , target_float
|
|
1462
|
+ #endif
|
|
1463
|
+ , fr_mm_s, extruder, millimeters
|
|
1464
|
+ )) {
|
|
1465
|
+ // Movement was not queued, probably because it was too short.
|
|
1466
|
+ // Simply accept that as movement queued and done
|
|
1467
|
+ return true;
|
|
1468
|
+ }
|
|
1469
|
+
|
|
1470
|
+ // Move buffer head
|
|
1471
|
+ block_buffer_head = next_buffer_head;
|
|
1472
|
+
|
|
1473
|
+ // Recalculate and optimize trapezoidal speed profiles
|
|
1474
|
+ recalculate();
|
|
1475
|
+
|
|
1476
|
+ // Movement successfully queued!
|
|
1477
|
+ return true;
|
|
1478
|
+}
|
|
1479
|
+
|
|
1480
|
+/**
|
|
1481
|
+ * Planner::_populate_block
|
|
1482
|
+ *
|
|
1483
|
+ * Fills a new linear movement in the block (in terms of steps).
|
1342
|
1484
|
*
|
1343
|
1485
|
* target - target position in steps units
|
1344
|
1486
|
* fr_mm_s - (target) speed of the move
|
1345
|
1487
|
* extruder - target extruder
|
|
1488
|
+ *
|
|
1489
|
+ * Returns true is movement is acceptable, false otherwise
|
1346
|
1490
|
*/
|
1347
|
|
-void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
|
1491
|
+bool Planner::_populate_block(block_t * const block, bool split_move,
|
|
1492
|
+ const int32_t (&target)[XYZE]
|
1348
|
1493
|
#if HAS_POSITION_FLOAT
|
1349
|
1494
|
, const float (&target_float)[XYZE]
|
1350
|
1495
|
#endif
|
|
@@ -1358,7 +1503,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1358
|
1503
|
int32_t de = target[E_AXIS] - position[E_AXIS];
|
1359
|
1504
|
|
1360
|
1505
|
/* <-- add a slash to enable
|
1361
|
|
- SERIAL_ECHOPAIR(" _buffer_steps FR:", fr_mm_s);
|
|
1506
|
+ SERIAL_ECHOPAIR(" _populate_block FR:", fr_mm_s);
|
1362
|
1507
|
SERIAL_ECHOPAIR(" A:", target[A_AXIS]);
|
1363
|
1508
|
SERIAL_ECHOPAIR(" (", da);
|
1364
|
1509
|
SERIAL_ECHOPAIR(" steps) B:", target[B_AXIS]);
|
|
@@ -1427,10 +1572,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1427
|
1572
|
const float esteps_float = de * e_factor[extruder];
|
1428
|
1573
|
const int32_t esteps = ABS(esteps_float) + 0.5;
|
1429
|
1574
|
|
1430
|
|
- // Wait for the next available block
|
1431
|
|
- uint8_t next_buffer_head;
|
1432
|
|
- block_t * const block = get_next_free_block(next_buffer_head);
|
1433
|
|
-
|
1434
|
1575
|
// Clear all flags, including the "busy" bit
|
1435
|
1576
|
block->flag = 0x00;
|
1436
|
1577
|
|
|
@@ -1466,7 +1607,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1466
|
1607
|
block->step_event_count = MAX4(block->steps[A_AXIS], block->steps[B_AXIS], block->steps[C_AXIS], esteps);
|
1467
|
1608
|
|
1468
|
1609
|
// Bail if this is a zero-length block
|
1469
|
|
- if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
|
|
1610
|
+ if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return false;
|
1470
|
1611
|
|
1471
|
1612
|
// For a mixing extruder, get a magnified step_event_count for each
|
1472
|
1613
|
#if ENABLED(MIXING_EXTRUDER)
|
|
@@ -1706,12 +1847,16 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1706
|
1847
|
#endif
|
1707
|
1848
|
|
1708
|
1849
|
#if ENABLED(ULTRA_LCD)
|
1709
|
|
- CRITICAL_SECTION_START
|
1710
|
|
- block_buffer_runtime_us += segment_time_us;
|
1711
|
|
- CRITICAL_SECTION_END
|
|
1850
|
+ // Protect the access to the position.
|
|
1851
|
+ const bool was_enabled = STEPPER_ISR_ENABLED();
|
|
1852
|
+ if (was_enabled) DISABLE_STEPPER_DRIVER_INTERRUPT();
|
|
1853
|
+
|
|
1854
|
+ block_buffer_runtime_us += segment_time_us;
|
|
1855
|
+
|
|
1856
|
+ if (was_enabled) ENABLE_STEPPER_DRIVER_INTERRUPT();
|
1712
|
1857
|
#endif
|
1713
|
1858
|
|
1714
|
|
- block->nominal_speed = block->millimeters * inverse_secs; // (mm/sec) Always > 0
|
|
1859
|
+ block->nominal_speed_sqr = sq(block->millimeters * inverse_secs); // (mm/sec)^2 Always > 0
|
1715
|
1860
|
block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
|
1716
|
1861
|
|
1717
|
1862
|
#if ENABLED(FILAMENT_WIDTH_SENSOR)
|
|
@@ -1799,8 +1944,8 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1799
|
1944
|
// Correct the speed
|
1800
|
1945
|
if (speed_factor < 1.0) {
|
1801
|
1946
|
LOOP_XYZE(i) current_speed[i] *= speed_factor;
|
1802
|
|
- block->nominal_speed *= speed_factor;
|
1803
|
1947
|
block->nominal_rate *= speed_factor;
|
|
1948
|
+ block->nominal_speed_sqr = block->nominal_speed_sqr * sq(speed_factor);
|
1804
|
1949
|
}
|
1805
|
1950
|
|
1806
|
1951
|
// Compute and limit the acceleration rate for the trapezoid generator.
|
|
@@ -1895,13 +2040,13 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1895
|
2040
|
block->acceleration_steps_per_s2 = accel;
|
1896
|
2041
|
block->acceleration = accel / steps_per_mm;
|
1897
|
2042
|
#if DISABLED(BEZIER_JERK_CONTROL)
|
1898
|
|
- block->acceleration_rate = (long)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
|
|
2043
|
+ block->acceleration_rate = (uint32_t)(accel * (4096.0 * 4096.0 / (HAL_STEPPER_TIMER_RATE)));
|
1899
|
2044
|
#endif
|
1900
|
2045
|
#if ENABLED(LIN_ADVANCE)
|
1901
|
2046
|
if (block->use_advance_lead) {
|
1902
|
2047
|
block->advance_speed = (HAL_STEPPER_TIMER_RATE) / (extruder_advance_K * block->e_D_ratio * block->acceleration * axis_steps_per_mm[E_AXIS_N]);
|
1903
|
2048
|
#if ENABLED(LA_DEBUG)
|
1904
|
|
- if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < block->nominal_speed * block->e_D_ratio)
|
|
2049
|
+ if (extruder_advance_K * block->e_D_ratio * block->acceleration * 2 < SQRT(block->nominal_speed_sqr) * block->e_D_ratio)
|
1905
|
2050
|
SERIAL_ECHOLNPGM("More than 2 steps per eISR loop executed.");
|
1906
|
2051
|
if (block->advance_speed < 200)
|
1907
|
2052
|
SERIAL_ECHOLNPGM("eISR running at > 10kHz.");
|
|
@@ -1909,7 +2054,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1909
|
2054
|
}
|
1910
|
2055
|
#endif
|
1911
|
2056
|
|
1912
|
|
- float vmax_junction; // Initial limit on the segment entry velocity
|
|
2057
|
+ float vmax_junction_sqr; // Initial limit on the segment entry velocity (mm/s)^2
|
1913
|
2058
|
|
1914
|
2059
|
#if ENABLED(JUNCTION_DEVIATION)
|
1915
|
2060
|
|
|
@@ -1935,7 +2080,17 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1935
|
2080
|
* changed dynamically during operation nor can the line move geometry. This must be kept in
|
1936
|
2081
|
* memory in the event of a feedrate override changing the nominal speeds of blocks, which can
|
1937
|
2082
|
* change the overall maximum entry speed conditions of all blocks.
|
1938
|
|
- */
|
|
2083
|
+ *
|
|
2084
|
+ * #######
|
|
2085
|
+ * https://github.com/MarlinFirmware/Marlin/issues/10341#issuecomment-388191754
|
|
2086
|
+ *
|
|
2087
|
+ * hoffbaked: on May 10 2018 tuned and improved the GRBL algorithm for Marlin:
|
|
2088
|
+ Okay! It seems to be working good. I somewhat arbitrarily cut it off at 1mm
|
|
2089
|
+ on then on anything with less sides than an octagon. With this, and the
|
|
2090
|
+ reverse pass actually recalculating things, a corner acceleration value
|
|
2091
|
+ of 1000 junction deviation of .05 are pretty reasonable. If the cycles
|
|
2092
|
+ can be spared, a better acos could be used. For all I know, it may be
|
|
2093
|
+ already calculated in a different place. */
|
1939
|
2094
|
|
1940
|
2095
|
// Unit vector of previous path line segment
|
1941
|
2096
|
static float previous_unit_vec[
|
|
@@ -1956,7 +2111,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1956
|
2111
|
};
|
1957
|
2112
|
|
1958
|
2113
|
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
|
1959
|
|
- if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
|
|
2114
|
+ if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
|
1960
|
2115
|
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
|
1961
|
2116
|
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
|
1962
|
2117
|
float junction_cos_theta = -previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
|
|
@@ -1970,21 +2125,33 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
1970
|
2125
|
// NOTE: Computed without any expensive trig, sin() or acos(), by trig half angle identity of cos(theta).
|
1971
|
2126
|
if (junction_cos_theta > 0.999999) {
|
1972
|
2127
|
// For a 0 degree acute junction, just set minimum junction speed.
|
1973
|
|
- vmax_junction = MINIMUM_PLANNER_SPEED;
|
|
2128
|
+ vmax_junction_sqr = sq(MINIMUM_PLANNER_SPEED);
|
1974
|
2129
|
}
|
1975
|
2130
|
else {
|
1976
|
|
- junction_cos_theta = MAX(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
|
|
2131
|
+ NOLESS(junction_cos_theta, -0.999999); // Check for numerical round-off to avoid divide by zero.
|
1977
|
2132
|
const float sin_theta_d2 = SQRT(0.5 * (1.0 - junction_cos_theta)); // Trig half angle identity. Always positive.
|
1978
|
2133
|
|
1979
|
2134
|
// TODO: Technically, the acceleration used in calculation needs to be limited by the minimum of the
|
1980
|
2135
|
// two junctions. However, this shouldn't be a significant problem except in extreme circumstances.
|
1981
|
|
- vmax_junction = SQRT((block->acceleration * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2));
|
|
2136
|
+ vmax_junction_sqr = (JUNCTION_ACCELERATION_FACTOR * JUNCTION_DEVIATION_FACTOR * sin_theta_d2) / (1.0 - sin_theta_d2);
|
|
2137
|
+ if (block->millimeters < 1.0) {
|
|
2138
|
+
|
|
2139
|
+ // Fast acos approximation, minus the error bar to be safe
|
|
2140
|
+ float junction_theta = (RADIANS(-40) * sq(junction_cos_theta) - RADIANS(50)) * junction_cos_theta + RADIANS(90) - 0.18;
|
|
2141
|
+
|
|
2142
|
+ // If angle is greater than 135 degrees (octagon), find speed for approximate arc
|
|
2143
|
+ if (junction_theta > RADIANS(135)) {
|
|
2144
|
+ const float limit_sqr = block->millimeters / (RADIANS(180) - junction_theta) * JUNCTION_ACCELERATION_FACTOR;
|
|
2145
|
+ NOMORE(vmax_junction_sqr, limit_sqr);
|
|
2146
|
+ }
|
|
2147
|
+ }
|
1982
|
2148
|
}
|
1983
|
2149
|
|
1984
|
|
- vmax_junction = MIN3(vmax_junction, block->nominal_speed, previous_nominal_speed);
|
|
2150
|
+ // Get the lowest speed
|
|
2151
|
+ vmax_junction_sqr = MIN3(vmax_junction_sqr, block->nominal_speed_sqr, previous_nominal_speed_sqr);
|
1985
|
2152
|
}
|
1986
|
2153
|
else // Init entry speed to zero. Assume it starts from rest. Planner will correct this later.
|
1987
|
|
- vmax_junction = 0.0;
|
|
2154
|
+ vmax_junction_sqr = 0.0;
|
1988
|
2155
|
|
1989
|
2156
|
COPY(previous_unit_vec, unit_vec);
|
1990
|
2157
|
|
|
@@ -2000,13 +2167,15 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
2000
|
2167
|
// Exit speed limited by a jerk to full halt of a previous last segment
|
2001
|
2168
|
static float previous_safe_speed;
|
2002
|
2169
|
|
2003
|
|
- float safe_speed = block->nominal_speed;
|
|
2170
|
+ const float nominal_speed = SQRT(block->nominal_speed_sqr);
|
|
2171
|
+ float safe_speed = nominal_speed;
|
|
2172
|
+
|
2004
|
2173
|
uint8_t limited = 0;
|
2005
|
2174
|
LOOP_XYZE(i) {
|
2006
|
2175
|
const float jerk = ABS(current_speed[i]), maxj = max_jerk[i];
|
2007
|
2176
|
if (jerk > maxj) {
|
2008
|
2177
|
if (limited) {
|
2009
|
|
- const float mjerk = maxj * block->nominal_speed;
|
|
2178
|
+ const float mjerk = maxj * nominal_speed;
|
2010
|
2179
|
if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
|
2011
|
2180
|
}
|
2012
|
2181
|
else {
|
|
@@ -2016,19 +2185,21 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
2016
|
2185
|
}
|
2017
|
2186
|
}
|
2018
|
2187
|
|
2019
|
|
- if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
|
|
2188
|
+ float vmax_junction;
|
|
2189
|
+ if (moves_queued && !UNEAR_ZERO(previous_nominal_speed_sqr)) {
|
2020
|
2190
|
// Estimate a maximum velocity allowed at a joint of two successive segments.
|
2021
|
2191
|
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
|
2022
|
2192
|
// then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
|
2023
|
2193
|
|
2024
|
|
- // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
|
2025
|
|
- // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
|
2026
|
|
- vmax_junction = MIN(block->nominal_speed, previous_nominal_speed);
|
2027
|
|
-
|
2028
|
2194
|
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
|
2029
|
2195
|
float v_factor = 1;
|
2030
|
2196
|
limited = 0;
|
2031
|
2197
|
|
|
2198
|
+ // The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
|
|
2199
|
+ // Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
|
|
2200
|
+ const float previous_nominal_speed = SQRT(previous_nominal_speed_sqr);
|
|
2201
|
+ vmax_junction = MIN(nominal_speed, previous_nominal_speed);
|
|
2202
|
+
|
2032
|
2203
|
// Now limit the jerk in all axes.
|
2033
|
2204
|
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
|
2034
|
2205
|
LOOP_XYZE(axis) {
|
|
@@ -2063,16 +2234,19 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
2063
|
2234
|
vmax_junction = safe_speed;
|
2064
|
2235
|
|
2065
|
2236
|
previous_safe_speed = safe_speed;
|
|
2237
|
+ vmax_junction_sqr = sq(vmax_junction);
|
|
2238
|
+
|
2066
|
2239
|
#endif // Classic Jerk Limiting
|
2067
|
2240
|
|
2068
|
2241
|
// Max entry speed of this block equals the max exit speed of the previous block.
|
2069
|
|
- block->max_entry_speed = vmax_junction;
|
|
2242
|
+ block->max_entry_speed_sqr = vmax_junction_sqr;
|
2070
|
2243
|
|
2071
|
2244
|
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
|
2072
|
|
- const float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
|
2073
|
|
- // If stepper ISR is disabled, this indicates buffer_segment wants to add a split block.
|
2074
|
|
- // In this case start with the max. allowed speed to avoid an interrupted first move.
|
2075
|
|
- block->entry_speed = STEPPER_ISR_ENABLED() ? MINIMUM_PLANNER_SPEED : MIN(vmax_junction, v_allowable);
|
|
2245
|
+ const float v_allowable_sqr = max_allowable_speed_sqr(-block->acceleration, sq(MINIMUM_PLANNER_SPEED), block->millimeters);
|
|
2246
|
+
|
|
2247
|
+ // If we are trying to add a split block, start with the
|
|
2248
|
+ // max. allowed speed to avoid an interrupted first move.
|
|
2249
|
+ block->entry_speed_sqr = !split_move ? sq(MINIMUM_PLANNER_SPEED) : MIN(vmax_junction_sqr, v_allowable_sqr);
|
2076
|
2250
|
|
2077
|
2251
|
// Initialize planner efficiency flags
|
2078
|
2252
|
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
|
|
@@ -2082,25 +2256,22 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE]
|
2082
|
2256
|
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
|
2083
|
2257
|
// the reverse and forward planners, the corresponding block junction speed will always be at the
|
2084
|
2258
|
// the maximum junction speed and may always be ignored for any speed reduction checks.
|
2085
|
|
- block->flag |= block->nominal_speed <= v_allowable ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
|
|
2259
|
+ block->flag |= block->nominal_speed_sqr <= v_allowable_sqr ? BLOCK_FLAG_RECALCULATE | BLOCK_FLAG_NOMINAL_LENGTH : BLOCK_FLAG_RECALCULATE;
|
2086
|
2260
|
|
2087
|
2261
|
// Update previous path unit_vector and nominal speed
|
2088
|
2262
|
COPY(previous_speed, current_speed);
|
2089
|
|
- previous_nominal_speed = block->nominal_speed;
|
2090
|
|
-
|
2091
|
|
- // Move buffer head
|
2092
|
|
- block_buffer_head = next_buffer_head;
|
|
2263
|
+ previous_nominal_speed_sqr = block->nominal_speed_sqr;
|
2093
|
2264
|
|
2094
|
|
- // Update the position (only when a move was queued)
|
|
2265
|
+ // Update the position
|
2095
|
2266
|
static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
|
2096
|
2267
|
COPY(position, target);
|
2097
|
2268
|
#if HAS_POSITION_FLOAT
|
2098
|
2269
|
COPY(position_float, target_float);
|
2099
|
2270
|
#endif
|
2100
|
2271
|
|
2101
|
|
- recalculate();
|
2102
|
|
-
|
2103
|
|
-} // _buffer_steps()
|
|
2272
|
+ // Movement was accepted
|
|
2273
|
+ return true;
|
|
2274
|
+} // _populate_block()
|
2104
|
2275
|
|
2105
|
2276
|
/**
|
2106
|
2277
|
* Planner::buffer_sync_block
|
|
@@ -2111,29 +2282,15 @@ void Planner::buffer_sync_block() {
|
2111
|
2282
|
uint8_t next_buffer_head;
|
2112
|
2283
|
block_t * const block = get_next_free_block(next_buffer_head);
|
2113
|
2284
|
|
2114
|
|
- block->flag = BLOCK_FLAG_SYNC_POSITION;
|
|
2285
|
+ // Clear block
|
|
2286
|
+ memset(block, 0, sizeof(block_t));
|
2115
|
2287
|
|
2116
|
|
- block->steps[A_AXIS] = position[A_AXIS];
|
2117
|
|
- block->steps[B_AXIS] = position[B_AXIS];
|
2118
|
|
- block->steps[C_AXIS] = position[C_AXIS];
|
2119
|
|
- block->steps[E_AXIS] = position[E_AXIS];
|
2120
|
|
-
|
2121
|
|
- #if ENABLED(LIN_ADVANCE)
|
2122
|
|
- block->use_advance_lead = false;
|
2123
|
|
- #endif
|
2124
|
|
-
|
2125
|
|
- block->nominal_speed =
|
2126
|
|
- block->entry_speed =
|
2127
|
|
- block->max_entry_speed =
|
2128
|
|
- block->millimeters =
|
2129
|
|
- block->acceleration = 0;
|
|
2288
|
+ block->flag = BLOCK_FLAG_SYNC_POSITION;
|
2130
|
2289
|
|
2131
|
|
- block->step_event_count =
|
2132
|
|
- block->nominal_rate =
|
2133
|
|
- block->initial_rate =
|
2134
|
|
- block->final_rate =
|
2135
|
|
- block->acceleration_steps_per_s2 =
|
2136
|
|
- block->segment_time_us = 0;
|
|
2290
|
+ block->position[A_AXIS] = position[A_AXIS];
|
|
2291
|
+ block->position[B_AXIS] = position[B_AXIS];
|
|
2292
|
+ block->position[C_AXIS] = position[C_AXIS];
|
|
2293
|
+ block->position[E_AXIS] = position[E_AXIS];
|
2137
|
2294
|
|
2138
|
2295
|
block_buffer_head = next_buffer_head;
|
2139
|
2296
|
stepper.wake_up();
|
|
@@ -2151,7 +2308,11 @@ void Planner::buffer_sync_block() {
|
2151
|
2308
|
* extruder - target extruder
|
2152
|
2309
|
* millimeters - the length of the movement, if known
|
2153
|
2310
|
*/
|
2154
|
|
-void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
|
|
2311
|
+bool Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder, const float &millimeters/*=0.0*/) {
|
|
2312
|
+
|
|
2313
|
+ // If we are cleaning, do not accept queuing of movements
|
|
2314
|
+ if (cleaning_buffer_counter) return false;
|
|
2315
|
+
|
2155
|
2316
|
// When changing extruders recalculate steps corresponding to the E position
|
2156
|
2317
|
#if ENABLED(DISTINCT_E_FACTORS)
|
2157
|
2318
|
if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) {
|
|
@@ -2220,37 +2381,80 @@ void Planner::buffer_segment(const float &a, const float &b, const float &c, con
|
2220
|
2381
|
const float between_float[ABCE] = { _BETWEEN_F(A), _BETWEEN_F(B), _BETWEEN_F(C), _BETWEEN_F(E) };
|
2221
|
2382
|
#endif
|
2222
|
2383
|
|
2223
|
|
- DISABLE_STEPPER_DRIVER_INTERRUPT();
|
|
2384
|
+ // The new head value is not assigned yet
|
|
2385
|
+ uint8_t buffer_head = 0;
|
|
2386
|
+ bool added = false;
|
2224
|
2387
|
|
2225
|
|
- _buffer_steps(between
|
2226
|
|
- #if HAS_POSITION_FLOAT
|
2227
|
|
- , between_float
|
2228
|
|
- #endif
|
2229
|
|
- , fr_mm_s, extruder, millimeters * 0.5
|
2230
|
|
- );
|
|
2388
|
+ uint8_t next_buffer_head;
|
|
2389
|
+ block_t *block = get_next_free_block(next_buffer_head, 2);
|
2231
|
2390
|
|
2232
|
|
- const uint8_t next = block_buffer_head;
|
|
2391
|
+ // Fill the block with the specified movement
|
|
2392
|
+ if (
|
|
2393
|
+ _populate_block(block, true, between
|
|
2394
|
+ #if HAS_POSITION_FLOAT
|
|
2395
|
+ , between_float
|
|
2396
|
+ #endif
|
|
2397
|
+ , fr_mm_s, extruder, millimeters * 0.5
|
|
2398
|
+ )
|
|
2399
|
+ ) {
|
|
2400
|
+ // Movement accepted - Point to the next reserved block
|
|
2401
|
+ block = &block_buffer[next_buffer_head];
|
|
2402
|
+
|
|
2403
|
+ // Store into the new to be stored head
|
|
2404
|
+ buffer_head = next_buffer_head;
|
|
2405
|
+ added = true;
|
|
2406
|
+
|
|
2407
|
+ // And advance the pointer to the next unused slot
|
|
2408
|
+ next_buffer_head = next_block_index(next_buffer_head);
|
|
2409
|
+ }
|
|
2410
|
+
|
|
2411
|
+ // Fill the second part of the block with the 2nd part of the movement
|
|
2412
|
+ if (
|
|
2413
|
+ _populate_block(block, true, target
|
|
2414
|
+ #if HAS_POSITION_FLOAT
|
|
2415
|
+ , target_float
|
|
2416
|
+ #endif
|
|
2417
|
+ , fr_mm_s, extruder, millimeters * 0.5
|
|
2418
|
+ )
|
|
2419
|
+ ) {
|
|
2420
|
+ // Movement accepted - If this block is a continuation
|
|
2421
|
+ // of the previous one, mark it as such
|
|
2422
|
+ if (added) SBI(block->flag, BLOCK_BIT_CONTINUED);
|
|
2423
|
+
|
|
2424
|
+ // Store into the new to be stored head
|
|
2425
|
+ buffer_head = next_buffer_head;
|
|
2426
|
+ added = true;
|
|
2427
|
+ }
|
2233
|
2428
|
|
2234
|
|
- _buffer_steps(target
|
|
2429
|
+ // If any of the movements was added
|
|
2430
|
+ if (added) {
|
|
2431
|
+
|
|
2432
|
+ // Move buffer head and add all the blocks that were filled
|
|
2433
|
+ // successfully to the movement queue.
|
|
2434
|
+ block_buffer_head = buffer_head;
|
|
2435
|
+
|
|
2436
|
+ // Update the position (only when a move was queued)
|
|
2437
|
+ static_assert(COUNT(target) > 1, "Parameter to _buffer_steps must be (&target)[XYZE]!");
|
|
2438
|
+ COPY(position, target);
|
2235
|
2439
|
#if HAS_POSITION_FLOAT
|
2236
|
|
- , target_float
|
|
2440
|
+ COPY(position_float, target_float);
|
2237
|
2441
|
#endif
|
2238
|
|
- , fr_mm_s, extruder, millimeters * 0.5
|
2239
|
|
- );
|
2240
|
2442
|
|
2241
|
|
- SBI(block_buffer[next].flag, BLOCK_BIT_CONTINUED);
|
2242
|
|
- ENABLE_STEPPER_DRIVER_INTERRUPT();
|
|
2443
|
+ // Recalculate and optimize trapezoidal speed profiles
|
|
2444
|
+ recalculate();
|
|
2445
|
+ }
|
2243
|
2446
|
}
|
2244
|
|
- else
|
2245
|
|
- _buffer_steps(target
|
|
2447
|
+ else if (
|
|
2448
|
+ !_buffer_steps(target
|
2246
|
2449
|
#if HAS_POSITION_FLOAT
|
2247
|
2450
|
, target_float
|
2248
|
2451
|
#endif
|
2249
|
2452
|
, fr_mm_s, extruder, millimeters
|
2250
|
|
- );
|
|
2453
|
+ )
|
|
2454
|
+ ) return false;
|
2251
|
2455
|
|
2252
|
2456
|
stepper.wake_up();
|
2253
|
|
-
|
|
2457
|
+ return true;
|
2254
|
2458
|
} // buffer_segment()
|
2255
|
2459
|
|
2256
|
2460
|
/**
|
|
@@ -2277,7 +2481,7 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c
|
2277
|
2481
|
position_float[C_AXIS] = c;
|
2278
|
2482
|
position_float[E_AXIS] = e;
|
2279
|
2483
|
#endif
|
2280
|
|
- previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
|
|
2484
|
+ previous_nominal_speed_sqr = 0.0; // Resets planner junction speeds. Assumes start from rest.
|
2281
|
2485
|
ZERO(previous_speed);
|
2282
|
2486
|
buffer_sync_block();
|
2283
|
2487
|
}
|
|
@@ -2298,22 +2502,6 @@ void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) {
|
2298
|
2502
|
}
|
2299
|
2503
|
|
2300
|
2504
|
/**
|
2301
|
|
- * Sync from the stepper positions. (e.g., after an interrupted move)
|
2302
|
|
- */
|
2303
|
|
-void Planner::sync_from_steppers() {
|
2304
|
|
- LOOP_XYZE(i) {
|
2305
|
|
- position[i] = stepper.position((AxisEnum)i);
|
2306
|
|
- #if HAS_POSITION_FLOAT
|
2307
|
|
- position_float[i] = position[i] * steps_to_mm[i
|
2308
|
|
- #if ENABLED(DISTINCT_E_FACTORS)
|
2309
|
|
- + (i == E_AXIS ? active_extruder : 0)
|
2310
|
|
- #endif
|
2311
|
|
- ];
|
2312
|
|
- #endif
|
2313
|
|
- }
|
2314
|
|
-}
|
2315
|
|
-
|
2316
|
|
-/**
|
2317
|
2505
|
* Setters for planner position (also setting stepper position).
|
2318
|
2506
|
*/
|
2319
|
2507
|
void Planner::set_position_mm(const AxisEnum axis, const float &v) {
|