MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Patch up reverse_pass_kernel (and other planner code) (#10674)

Browse Source
This commit is contained in:
Scott Lahteine 2018-05-10 01:30:55 -05:00 committed by GitHub
parent fe5c308872
commit 439e0cdd0f
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 37 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
Marlin/src/module/planner.cpp
View File

@ -821,21 +821,24 @@ void Planner::calculate_trapezoid_for_block(block_t* const block, const float &e
// POW((before->speed_x-after->speed_x), 2)+POW((before->speed_y-after->speed_y), 2)); // POW((before->speed_x-after->speed_x), 2)+POW((before->speed_y-after->speed_y), 2));
//} //}
// The kernel called by recalculate() when scanning the plan from last to first entry. // The kernel called by recalculate() when scanning the plan from last to first entry.
void Planner::reverse_pass_kernel(block_t* const current, const block_t * const next) { void Planner::reverse_pass_kernel(block_t* const current, const block_t* const next) {
if (!current || !next) return; if (current && next) {
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
// check for maximum allowable speed reductions to ensure maximum possible planned speed. // check for maximum allowable speed reductions to ensure maximum possible planned speed.
float max_entry_speed = current->max_entry_speed; const float max_entry_speed = current->max_entry_speed;
if (current->entry_speed != max_entry_speed) { if (current->entry_speed != max_entry_speed || TEST(next->flag, BLOCK_BIT_RECALCULATE)) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute // If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false. // for max allowable speed if block is decelerating and nominal length is false.
current->entry_speed = (TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed) const float new_entry_speed = (TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
? max_entry_speed ? max_entry_speed
: min(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters)); : min(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
SBI(current->flag, BLOCK_BIT_RECALCULATE); if (new_entry_speed != current->entry_speed) {
current->entry_speed = new_entry_speed;
SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
}
} }
} }
@ -845,7 +848,7 @@ void Planner::reverse_pass_kernel(block_t* const current, const block_t * const
*/ */
void Planner::reverse_pass() { void Planner::reverse_pass() {
if (movesplanned() > 2) { if (movesplanned() > 2) {
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. 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.
uint8_t blocknr = prev_block_index(block_buffer_head); uint8_t blocknr = prev_block_index(block_buffer_head);
block_t* current = &block_buffer[blocknr]; block_t* current = &block_buffer[blocknr];
@ -854,10 +857,13 @@ void Planner::reverse_pass() {
if (current->entry_speed != max_entry_speed) { if (current->entry_speed != max_entry_speed) {
// If nominal length true, max junction speed is guaranteed to be reached. Only compute // If nominal length true, max junction speed is guaranteed to be reached. Only compute
// for max allowable speed if block is decelerating and nominal length is false. // for max allowable speed if block is decelerating and nominal length is false.
current->entry_speed = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH) const float new_entry_speed = TEST(current->flag, BLOCK_BIT_NOMINAL_LENGTH)
? max_entry_speed ? max_entry_speed
: min(max_entry_speed, max_allowable_speed(-current->acceleration, MINIMUM_PLANNER_SPEED, current->millimeters)); : min(max_entry_speed, max_allowable_speed(-current->acceleration, MINIMUM_PLANNER_SPEED, current->millimeters));
SBI(current->flag, BLOCK_BIT_RECALCULATE); if (current->entry_speed != new_entry_speed) {
current->entry_speed = new_entry_speed;
SBI(current->flag, BLOCK_BIT_RECALCULATE);
}
} }
do { do {
@ -870,21 +876,20 @@ void Planner::reverse_pass() {
} }
// The kernel called by recalculate() when scanning the plan from first to last entry. // The kernel called by recalculate() when scanning the plan from first to last entry.
void Planner::forward_pass_kernel(const block_t * const previous, block_t* const current) { void Planner::forward_pass_kernel(const block_t* const previous, block_t* const current) {
if (!previous) return; if (previous) {
// If the previous block is an acceleration block, too short to complete the full speed
// If the previous block is an acceleration block, but it is not long enough to complete the // change, adjust the entry speed accordingly. Entry speeds have already been reset,
// full speed change within the block, we need to adjust the entry speed accordingly. Entry // maximized, and reverse-planned. If nominal length is set, max junction speed is
// speeds have already been reset, maximized, and reverse planned by reverse planner. // guaranteed to be reached. No need to recheck.
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck. if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) {
if (!TEST(previous->flag, BLOCK_BIT_NOMINAL_LENGTH)) { if (previous->entry_speed < current->entry_speed) {
if (previous->entry_speed < current->entry_speed) { const float new_entry_speed = min(current->entry_speed, max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
float entry_speed = min(current->entry_speed, // Check for junction speed change
max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters)); if (current->entry_speed != new_entry_speed) {
// Check for junction speed change current->entry_speed = new_entry_speed;
if (current->entry_speed != entry_speed) { SBI(current->flag, BLOCK_BIT_RECALCULATE);
current->entry_speed = entry_speed; }
SBI(current->flag, BLOCK_BIT_RECALCULATE);
} }
} }
} }