void Planner::plan_move(const std::vector<int>& steps,
float length,
float speed,
float acceleration,
float entry_speed)
{
PlanBlock *block = &block_buffer[block_buffer_head];
block->move.steps = steps;
block->move.length = length;
block->move.speed = speed;
block->move.acceleration = acceleration;
block->entry_speed_sqr = 0;
block->nominal_speed_sqr = speed*speed;
block->max_change_speed_sqr = 2*length*acceleration;
if (block_buffer_head == block_buffer_tail) {
block->max_entry_speed_sqr = 0;
}
else {
// Not first block, compute entry speed
float prev_nominal_speed_sqr =
block_buffer[prev_block_index(block_buffer_head)].nominal_speed_sqr;
block->max_entry_speed_sqr = std::min(std::min(entry_speed*entry_speed,
block->nominal_speed_sqr),
prev_nominal_speed_sqr);
}
block_buffer_head = next_buffer_head;
next_buffer_head = next_block_index(block_buffer_head);
// Finish up by recalculating the plan with the new block.
recalculate();
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass.
static void planner_reverse_pass() {
auto int8_t block_index = block_buffer_head;
block_t *block[3] = {NULL, NULL, NULL};
while(block_index != block_buffer_tail) {
block_index = prev_block_index( block_index );
block[2]= block[1];
block[1]= block[0];
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
}
// Skip buffer tail/first block to prevent over-writing the initial entry speed.
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass.
void planner_reverse_pass() {
char block_index = block_buffer_head;
if(((block_buffer_head-block_buffer_tail + BLOCK_BUFFER_SIZE) & (BLOCK_BUFFER_SIZE - 1)) > 3) {
block_index = (block_buffer_head - 3) & (BLOCK_BUFFER_SIZE - 1);
block_t *block[3] = { NULL, NULL, NULL };
while(block_index != block_buffer_tail) {
block_index = prev_block_index(block_index);
block[2]= block[1];
block[1]= block[0];
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
}
}
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass.
void planner_reverse_pass() {
uint8_t block_index = block_buffer_head;
//Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START;
unsigned char tail = block_buffer_tail;
CRITICAL_SECTION_END
if (BLOCK_MOD(block_buffer_head - tail + BLOCK_BUFFER_SIZE) > 3) { // moves queued
block_t *block[3] = { NULL, NULL, NULL };
while (block_index != tail) {
block_index = prev_block_index(block_index);
block[2]= block[1];
block[1]= block[0];
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
}
}
}
// planner_recalculate() needs to go over the current plan twice. Once in reverse and once forward. This
// implements the reverse pass.
void planner_reverse_pass() {
uint8_t block_index = block_buffer_head;
//Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START;
unsigned char tail = block_buffer_tail;
CRITICAL_SECTION_END;
if(((block_buffer_head - tail + block_buffer_size) & block_buffer_mask) > 3) {
block_index = (block_buffer_head - 3) & block_buffer_mask;
block_t *block[3] = { NULL, NULL, NULL };
while(block_index != tail) {
block_index = prev_block_index(block_index);
block[2]= block[1];
block[1]= block[0];
block[0] = &block_buffer[block_index];
planner_reverse_pass_kernel(block[0], block[1], block[2]);
}
}
}
/**
* recalculate() needs to go over the current plan twice.
* Once in reverse and once forward. This implements the reverse pass.
*/
void Planner::reverse_pass() {
if (movesplanned() > 3) {
block_t* block[3] = { NULL, NULL, NULL };
// Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START;
uint8_t tail = block_buffer_tail;
CRITICAL_SECTION_END
uint8_t b = BLOCK_MOD(block_buffer_head - 3);
while (b != tail) {
b = prev_block_index(b);
block[2] = block[1];
block[1] = block[0];
block[0] = &block_buffer[b];
reverse_pass_kernel(block[0], block[1], block[2]);
}
}
}
// planner, called whenever a new block was added
// All planner computations are performed with doubles (float on Arduinos) to minimize numerical round-
// off errors. Only when planned values are converted to stepper rate parameters, these are integers.
inline static void planner_recalculate() {
//// reverse pass
// Recalculate entry_speed to be (a) less or equal to vmax_junction and
// (b) low enough so it can definitely reach the next entry_speed at fixed acceleration.
int8_t block_index = block_buffer_head;
block_t *previous = NULL; // block closer to tail (older)
block_t *current = NULL; // block who's entry_speed to be adjusted
block_t *next = NULL; // block closer to head (newer)
while(block_index != block_buffer_tail) {
block_index = prev_block_index( block_index );
next = current;
current = previous;
previous = &block_buffer[block_index];
if (current && next) {
reduce_entry_speed_reverse(current, next);
}
} // skip tail/first block
//// forward pass
// Recalculate entry_speed to be low enough it can definitely
// be reached from previous entry_speed at fixed acceleration.
block_index = block_buffer_tail;
previous = NULL; // block closer to tail (older)
current = NULL; // block who's entry_speed to be adjusted
next = NULL; // block closer to head (newer)
while(block_index != block_buffer_head) {
previous = current;
current = next;
next = &block_buffer[block_index];
if (previous && current) {
reduce_entry_speed_forward(previous, current);
}
block_index = next_block_index(block_index);
}
if (current && next) {
reduce_entry_speed_forward(current, next);
}
//// recalculate trapeziods for all flagged blocks
// At this point all blocks have entry_speeds that that can be (a) reached from the prevous
// entry_speed with the one and only acceleration from our settings and (b) have junction
// speeds that do not exceed our limits for given direction change.
// Now we only need to calculate the actual accelerate_until and decelerate_after values.
block_index = block_buffer_tail;
current = NULL;
next = NULL;
while(block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
if (current) {
if (current->recalculate_flag || next->recalculate_flag) {
calculate_trapezoid_for_block( current,
current->entry_speed/current->nominal_speed,
next->entry_speed/current->nominal_speed );
current->recalculate_flag = false;
}
}
block_index = next_block_index( block_index );
}
// always recalculate last (newest) block with zero exit speed
calculate_trapezoid_for_block( next,
next->entry_speed/next->nominal_speed, ZERO_SPEED/next->nominal_speed );
next->recalculate_flag = false;
}
block_t *plan_get_recent_block() {
if (get_block_buffer_head() == get_block_buffer_tail()) { return(NULL); }
return(get_block_buffer()+prev_block_index(get_block_buffer_head()));
}
void Planner::recalculate()
{
// Initialize block index to the last block in the planner buffer.
std::size_t block_index = prev_block_index(block_buffer_head);
// Bail. Can't do anything with one only one plan-able block.
if (block_index == block_buffer_planned) {
return;
}
// Reverse Pass: Coarsely maximize all possible deceleration curves back-planning from the last
// block in buffer. Cease planning when the last optimal planned or tail pointer is reached.
// NOTE: Forward pass will later refine and correct the reverse pass to create an optimal plan.
float entry_speed_sqr;
PlanBlock *next;
PlanBlock *current = &block_buffer[block_index];
// Calculate maximum entry speed for last block in buffer, where the exit speed is always zero.
current->entry_speed_sqr = std::min(current->max_entry_speed_sqr,
current->max_change_speed_sqr);
block_index = prev_block_index(block_index);
if (block_index == block_buffer_planned) {
// Only two plannable blocks in buffer. Reverse pass complete.
// Check if the first block is the tail. If so, notify stepper to update its current parameters.
if (block_index == block_buffer_tail) {
// stepper->update_plan_block_parameters();
}
}
else { // Three or more plan-able blocks
while (block_index != block_buffer_planned) {
next = current;
current = &block_buffer[block_index];
block_index = prev_block_index(block_index);
// Check if next block is the tail block(=planned block). If so, update current stepper parameters.
if (block_index == block_buffer_tail) {
// stepper->update_plan_block_parameters();
}
// Compute maximum entry speed decelerating over the current block from its exit speed.
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
entry_speed_sqr = next->entry_speed_sqr + current->max_change_speed_sqr;
if (entry_speed_sqr < current->max_entry_speed_sqr) {
current->entry_speed_sqr = entry_speed_sqr;
}
else {
current->entry_speed_sqr = current->max_entry_speed_sqr;
}
}
}
}
// Forward Pass: Forward plan the acceleration curve from the planned pointer onward.
// Also scans for optimal plan breakpoints and appropriately updates the planned pointer.
next = &block_buffer[block_buffer_planned]; // Begin at buffer planned pointer
block_index = next_block_index(block_buffer_planned);
while (block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
// Any acceleration detected in the forward pass automatically moves the optimal planned
// pointer forward, since everything before this is all optimal. In other words, nothing
// can improve the plan from the buffer tail to the planned pointer by logic.
if (current->entry_speed_sqr < next->entry_speed_sqr) {
entry_speed_sqr = current->entry_speed_sqr + current->max_change_speed_sqr;
// If true, current block is full-acceleration and we can move the planned pointer forward.
if (entry_speed_sqr < next->entry_speed_sqr) {
next->entry_speed_sqr = entry_speed_sqr; // Always <= max_entry_speed_sqr. Backward pass sets this.
block_buffer_planned = block_index; // Set optimal plan pointer.
}
}
// Any block set at its maximum entry speed also creates an optimal plan up to this
// point in the buffer. When the plan is bracketed by either the beginning of the
// buffer and a maximum entry speed or two maximum entry speeds, every block in between
// cannot logically be further improved. Hence, we don't have to recompute them anymore.
if (next->entry_speed_sqr == next->max_entry_speed_sqr) {
block_buffer_planned = block_index;
}
block_index = next_block_index( block_index );
}
}
inline block_t *plan_get_recent_block()
{
if (block_buffer_head == block_buffer_tail) { return(NULL); }
return(&block_buffer[prev_block_index(block_buffer_head)]);
}
/* PLANNER SPEED DEFINITION
+--------+ <- current->nominal_speed
/ \
current->entry_speed -> + \
| + <- next->entry_speed
+-------------+
time -->
Recalculates the motion plan according to the following algorithm:
1. Go over every block in reverse order and calculate a junction speed reduction (i.e. block_t.entry_speed)
so that:
a. The junction speed is equal to or less than the maximum junction speed limit
b. No speed reduction within one block requires faster deceleration than the acceleration limits.
c. The last (or newest appended) block is planned from a complete stop.
2. Go over every block in chronological (forward) order and dial down junction speed values if
a. The speed increase within one block would require faster acceleration than the acceleration limits.
When these stages are complete, all blocks have a junction entry speed that will allow all speed changes
to be performed using the overall limiting acceleration value, and where no junction speed is greater
than the max limit. In other words, it just computed the fastest possible velocity profile through all
buffered blocks, where the final buffered block is planned to come to a full stop when the buffer is fully
executed. Finally it will:
3. Convert the plan to data that the stepper algorithm needs. Only block trapezoids adjacent to a
a planner-modified junction speed with be updated, the others are assumed ok as is.
All planner computations(1)(2) are performed in floating point to minimize numerical round-off errors. Only
when planned values are converted to stepper rate parameters(3), these are integers. If another motion block
is added while executing, the planner will re-plan and update the stored optimal velocity profile as it goes.
Conceptually, the planner works like blowing up a balloon, where the balloon is the velocity profile. It's
constrained by the speeds at the beginning and end of the buffer, along with the maximum junction speeds and
nominal speeds of each block. Once a plan is computed, or balloon filled, this is the optimal velocity profile
through all of the motions in the buffer. Whenever a new block is added, this changes some of the limiting
conditions, or how the balloon is filled, so it has to be re-calculated to get the new optimal velocity profile.
Also, since the planner only computes on what's in the planner buffer, some motions with lots of short line
segments, like arcs, may seem to move slow. This is because there simply isn't enough combined distance traveled
in the entire buffer to accelerate up to the nominal speed and then decelerate to a stop at the end of the
buffer. There are a few simple solutions to this: (1) Maximize the machine acceleration. The planner will be
able to compute higher speed profiles within the same combined distance. (2) Increase line segment(s) distance.
The more combined distance the planner has to use, the faster it can go. (3) Increase the MINIMUM_PLANNER_SPEED.
Not recommended. This will change what speed the planner plans to at the end of the buffer. Can lead to lost
steps when coming to a stop. (4) [BEST] Increase the planner buffer size. The more combined distance, the
bigger the balloon, or faster it can go. But this is not possible for 328p Arduinos because its limited memory
is already maxed out. Future ARM versions should not have this issue, with look-ahead planner blocks numbering
up to a hundred or more.
NOTE: Since this function is constantly re-calculating for every new incoming block, it must be as efficient
as possible. For example, in situations like arc generation or complex curves, the short, rapid line segments
can execute faster than new blocks can be added, and the planner buffer will then starve and empty, leading
to weird hiccup-like jerky motions.
*/
static void planner_recalculate()
{
// float entry_speed_sqr;
// uint8_t block_index = block_buffer_head;
// block_t *previous = NULL;
// block_t *current = NULL;
// block_t *next;
// while (block_index != block_buffer_tail) {
// block_index = prev_block_index( block_index );
// next = current;
// current = previous;
// previous = &block_buffer[block_index];
//
// if (next && current) {
// if (next != block_buffer_planned) {
// if (previous == block_buffer_tail) { block_buffer_planned = next; }
// else {
//
// if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
// current->recalculate_flag = true; // Almost always changes. So force recalculate.
// entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
// if (entry_speed_sqr < current->max_entry_speed_sqr) {
// current->entry_speed_sqr = entry_speed_sqr;
// } else {
// current->entry_speed_sqr = current->max_entry_speed_sqr;
// }
// } else {
// block_buffer_planned = current;
// }
// }
// } else {
// break;
// }
// }
// }
//
// block_index = block_buffer_planned;
// next = &block_buffer[block_index];
// current = prev_block_index(block_index);
// while (block_index != block_buffer_head) {
//
// // If the current block is an acceleration block, but it is not long enough to complete the
// // full speed change within the block, we need to adjust the exit speed accordingly. Entry
// // speeds have already been reset, maximized, and reverse planned by reverse planner.
// if (current->entry_speed_sqr < next->entry_speed_sqr) {
// // Compute block exit speed based on the current block speed and distance
// // Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance
// entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
//
// // If it's less than the stored value, update the exit speed and set recalculate flag.
// if (entry_speed_sqr < next->entry_speed_sqr) {
// next->entry_speed_sqr = entry_speed_sqr;
// next->recalculate_flag = true;
// }
// }
//
// // Recalculate if current block entry or exit junction speed has changed.
// if (current->recalculate_flag || next->recalculate_flag) {
// // NOTE: Entry and exit factors always > 0 by all previous logic operations.
// calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
// current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
// }
//
// current = next;
// next = &block_buffer[block_index];
// block_index = next_block_index( block_index );
// }
//
// // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
// calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
// next->recalculate_flag = false;
// TODO: No over-write protection exists for the executing block. For most cases this has proven to be ok, but
// for feed-rate overrides, something like this is essential. Place a request here to the stepper driver to
// find out where in the planner buffer is the a safe place to begin re-planning from.
// if (block_buffer_head != block_buffer_tail) {
float entry_speed_sqr;
// Perform reverse planner pass. Skip the head(end) block since it is already initialized, and skip the
// tail(first) block to prevent over-writing of the initial entry speed.
uint8_t block_index = prev_block_index( block_buffer_head ); // Assume buffer is not empty.
block_t *current = &block_buffer[block_index]; // Head block-1 = Newly appended block
block_t *next;
if (block_index != block_buffer_tail) { block_index = prev_block_index( block_index ); }
while (block_index != block_buffer_tail) {
next = current;
current = &block_buffer[block_index];
// TODO: Determine maximum entry speed at junction for feedrate overrides, since they can alter
// the planner nominal speeds at any time. This calc could be done in the override handler, but
// this could require an additional variable to be stored to differentiate the programmed nominal
// speeds, max junction speed, and override speeds/scalar.
// 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
// check for maximum allowable speed reductions to ensure maximum possible planned speed.
if (current->entry_speed_sqr != current->max_entry_speed_sqr) {
current->entry_speed_sqr = current->max_entry_speed_sqr;
current->recalculate_flag = true; // Almost always changes. So force recalculate.
if (next->entry_speed_sqr < current->max_entry_speed_sqr) {
// Computes: v_entry^2 = v_exit^2 + 2*acceleration*distance
entry_speed_sqr = next->entry_speed_sqr + 2*current->acceleration*current->millimeters;
if (entry_speed_sqr < current->max_entry_speed_sqr) {
current->entry_speed_sqr = entry_speed_sqr;
}
}
}
block_index = prev_block_index( block_index );
}
// Perform forward planner pass. Begins junction speed adjustments after tail(first) block.
// Also recalculate trapezoids, block by block, as the forward pass completes the plan.
block_index = next_block_index(block_buffer_tail);
next = &block_buffer[block_buffer_tail]; // Places tail(first) block into current
while (block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
// If the current block is an acceleration block, but it is not long enough to complete the
// full speed change within the block, we need to adjust the exit speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner.
if (current->entry_speed_sqr < next->entry_speed_sqr) {
// Compute block exit speed based on the current block speed and distance
// Computes: v_exit^2 = v_entry^2 + 2*acceleration*distance
entry_speed_sqr = current->entry_speed_sqr + 2*current->acceleration*current->millimeters;
// If it's less than the stored value, update the exit speed and set recalculate flag.
if (entry_speed_sqr < next->entry_speed_sqr) {
next->entry_speed_sqr = entry_speed_sqr;
next->recalculate_flag = true;
}
}
// Recalculate if current block entry or exit junction speed has changed.
if (current->recalculate_flag || next->recalculate_flag) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
calculate_trapezoid_for_block(current, current->entry_speed_sqr, next->entry_speed_sqr);
current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
}
block_index = next_block_index( block_index );
}
// Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
calculate_trapezoid_for_block(next, next->entry_speed_sqr, MINIMUM_PLANNER_SPEED*MINIMUM_PLANNER_SPEED);
next->recalculate_flag = false;
// }
}
