|
@@ -821,21 +821,24 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
|
821
|
821
|
// POW((before->speed_x-after->speed_x), 2)+POW((before->speed_y-after->speed_y), 2));
|
822
|
822
|
//}
|
823
|
823
|
|
824
|
|
-
|
825
|
824
|
// The kernel called by recalculate() when scanning the plan from last to first entry.
|
826
|
|
-void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) {
|
827
|
|
- if (!current || !next) return;
|
828
|
|
- // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
|
829
|
|
- // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
|
830
|
|
- // check for maximum allowable speed reductions to ensure maximum possible planned speed.
|
831
|
|
- float max_entry_speed = current->max_entry_speed;
|
832
|
|
- if (current->entry_speed != max_entry_speed) {
|
833
|
|
- // If nominal length true, max junction speed is guaranteed to be reached. Only compute
|
834
|
|
- // for max allowable speed if block is decelerating and nominal length is false.
|
835
|
|
- current->entry_speed = (TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
|
836
|
|
- ? max_entry_speed
|
837
|
|
- : min(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
|
838
|
|
- SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
|
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;
|
|
839
|
+ SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
|
840
|
+ }
|
|
841
|
+ }
|
839
|
842
|
}
|
840
|
843
|
}
|
841
|
844
|
|
|
@@ -845,7 +848,7 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const
|
845
|
848
|
*/
|
846
|
849
|
void Planner::reverse_pass() {
|
847
|
850
|
if (movesplanned() > 2) {
|
848
|
|
- const uint8_t endnr = BLOCK_MOD(block_buffer_tail + 1); // tail is running. tail+1 shouldn't be altered because it's connected to the running block.
|
|
851
|
+ 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.
|
849
|
852
|
uint8_t blocknr = prev_block_index(block_buffer_head);
|
850
|
853
|
block_t* current = &block_buffer[blocknr];
|
851
|
854
|
|
|
@@ -854,10 +857,13 @@ void Planner::reverse_pass() {
|
854
|
857
|
if (current->entry_speed != max_entry_speed) {
|
855
|
858
|
// If nominal length true, max junction speed is guaranteed to be reached. Only compute
|
856
|
859
|
// for max allowable speed if block is decelerating and nominal length is false.
|
857
|
|
- current->entry_speed = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
|
|
860
|
+ const float new_entry_speed = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
|
858
|
861
|
? max_entry_speed
|
859
|
862
|
: min(max_entry_speed, max_allowable_speed(-current->acceleration, MINIMUM_PLANNER_SPEED, current->millimeters));
|
860
|
|
- SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
|
863
|
+ if (current->entry_speed != new_entry_speed) {
|
|
864
|
+ current->entry_speed = new_entry_speed;
|
|
865
|
+ SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
|
866
|
+ }
|
861
|
867
|
}
|
862
|
868
|
|
863
|
869
|
do {
|
|
@@ -870,21 +876,20 @@ void Planner::reverse_pass() {
|
870
|
876
|
}
|
871
|
877
|
|
872
|
878
|
// The kernel called by recalculate() when scanning the plan from first to last entry.
|
873
|
|
-void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current) {
|
874
|
|
- if (!previous) return;
|
875
|
|
-
|
876
|
|
- // If the previous block is an acceleration block, but it is not long enough to complete the
|
877
|
|
- // full speed change within the block, we need to adjust the entry speed accordingly. Entry
|
878
|
|
- // speeds have already been reset, maximized, and reverse planned by reverse planner.
|
879
|
|
- // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
|
880
|
|
- if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) {
|
881
|
|
- if (previous->entry_speed < current->entry_speed) {
|
882
|
|
- float entry_speed = min(current->entry_speed,
|
883
|
|
- max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
|
884
|
|
- // Check for junction speed change
|
885
|
|
- if (current->entry_speed != entry_speed) {
|
886
|
|
- current->entry_speed = entry_speed;
|
887
|
|
- SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
|
879
|
+void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current) {
|
|
880
|
+ if (previous) {
|
|
881
|
+ // If the previous block is an acceleration block, too short to complete the full speed
|
|
882
|
+ // change, adjust the entry speed accordingly. Entry speeds have already been reset,
|
|
883
|
+ // maximized, and reverse-planned. If nominal length is set, max junction speed is
|
|
884
|
+ // guaranteed to be reached. No need to recheck.
|
|
885
|
+ 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;
|
|
891
|
+ SBI(current->flag, BLOCK_BIT_RECALCULATE);
|
|
892
|
+ }
|
888
|
893
|
}
|
889
|
894
|
}
|
890
|
895
|
}
|