Replace block flag bools with flag bits

…and apply const to some method parameters
This commit is contained in:
Scott Lahteine 2016-10-10 23:17:49 -05:00
parent d41f2bdbd8
commit 8e1cc9332a
2 changed files with 51 additions and 48 deletions

View file

@ -149,7 +149,7 @@ void Planner::init() {
* Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors.
*/
void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor) {
void Planner::calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor) {
uint32_t initial_rate = ceil(block->nominal_rate * entry_factor),
final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second)
@ -203,29 +203,20 @@ void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor,
// The kernel called by recalculate() when scanning the plan from last to first entry.
void Planner::reverse_pass_kernel(block_t* current, block_t* next) {
if (!current) return;
if (next) {
// 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.
float max_entry_speed = current->max_entry_speed;
if (current->entry_speed != max_entry_speed) {
// 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.
if (!current->nominal_length_flag && max_entry_speed > next->entry_speed) {
current->entry_speed = min(max_entry_speed,
max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
}
else {
current->entry_speed = max_entry_speed;
}
current->recalculate_flag = true;
}
} // Skip last block. Already initialized and set for recalculation.
void Planner::reverse_pass_kernel(block_t* const current, const block_t *next) {
if (!current || !next) return;
// 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.
float max_entry_speed = current->max_entry_speed;
if (current->entry_speed != max_entry_speed) {
// 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.
current->entry_speed = ((current->flag & BLOCK_FLAG_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
? max_entry_speed
: min(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
current->flag |= BLOCK_FLAG_RECALCULATE;
}
}
/**
@ -255,21 +246,21 @@ void Planner::reverse_pass() {
}
// The kernel called by recalculate() when scanning the plan from first to last entry.
void Planner::forward_pass_kernel(block_t* previous, block_t* current) {
void Planner::forward_pass_kernel(const block_t* previous, block_t* const current) {
if (!previous) return;
// If the previous 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 entry speed accordingly. Entry
// speeds have already been reset, maximized, and reverse planned by reverse planner.
// If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
if (!previous->nominal_length_flag) {
if (!(previous->flag & BLOCK_FLAG_NOMINAL_LENGTH)) {
if (previous->entry_speed < current->entry_speed) {
float entry_speed = min(current->entry_speed,
max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
// Check for junction speed change
if (current->entry_speed != entry_speed) {
current->entry_speed = entry_speed;
current->recalculate_flag = true;
current->flag |= BLOCK_FLAG_RECALCULATE;
}
}
}
@ -298,19 +289,18 @@ void Planner::forward_pass() {
*/
void Planner::recalculate_trapezoids() {
int8_t block_index = block_buffer_tail;
block_t* current;
block_t* next = NULL;
block_t *current, *next = NULL;
while (block_index != block_buffer_head) {
current = next;
next = &block_buffer[block_index];
if (current) {
// Recalculate if current block entry or exit junction speed has changed.
if (current->recalculate_flag || next->recalculate_flag) {
if ((current->flag & BLOCK_FLAG_RECALCULATE) || (next->flag & BLOCK_FLAG_RECALCULATE)) {
// NOTE: Entry and exit factors always > 0 by all previous logic operations.
float nom = current->nominal_speed;
calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom);
current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
current->flag &= ~BLOCK_FLAG_RECALCULATE; // Reset current only to ensure next trapezoid is computed
}
}
block_index = next_block_index(block_index);
@ -319,7 +309,7 @@ void Planner::recalculate_trapezoids() {
if (next) {
float nom = next->nominal_speed;
calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom);
next->recalculate_flag = false;
next->flag &= ~BLOCK_FLAG_RECALCULATE;
}
}
@ -1119,8 +1109,9 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
block->flag &= ~BLOCK_FLAG_NOMINAL_LENGTH;
if (block->nominal_speed <= v_allowable) block->flag |= BLOCK_FLAG_NOMINAL_LENGTH;
block->flag |= BLOCK_FLAG_RECALCULATE; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed));

View file

@ -40,6 +40,19 @@
#include "vector_3.h"
#endif
enum BlockFlag {
// Recalculate trapezoids on entry junction. For optimization.
BLOCK_FLAG_RECALCULATE = _BV(0),
// Nominal speed always reached.
// i.e., The segment is long enough, so the nominal speed is reachable if accelerating
// from a safe speed (in consideration of jerking from zero speed).
BLOCK_FLAG_NOMINAL_LENGTH = _BV(1),
// Start from a halt at the start of this block, respecting the maximum allowed jerk.
BLOCK_FLAG_START_FROM_FULL_HALT = _BV(2)
};
/**
* struct block_t
*
@ -79,19 +92,18 @@ typedef struct {
#endif
// Fields used by the motion planner to manage acceleration
float nominal_speed, // The nominal speed for this block in mm/sec
entry_speed, // Entry speed at previous-current junction in mm/sec
max_entry_speed, // Maximum allowable junction entry speed in mm/sec
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2
unsigned char recalculate_flag, // Planner flag to recalculate trapezoids on entry junction
nominal_length_flag; // Planner flag for nominal speed always reached
float nominal_speed, // The nominal speed for this block in mm/sec
entry_speed, // Entry speed at previous-current junction in mm/sec
max_entry_speed, // Maximum allowable junction entry speed in mm/sec
millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2
uint8_t flag; // Block flags (See BlockFlag enum above)
// Settings for the trapezoid generator
unsigned long nominal_rate, // The nominal step rate for this block in step_events/sec
initial_rate, // The jerk-adjusted step rate at start of block
final_rate, // The minimal rate at exit
acceleration_steps_per_s2; // acceleration steps/sec^2
uint32_t nominal_rate, // The nominal step rate for this block in step_events/sec
initial_rate, // The jerk-adjusted step rate at start of block
final_rate, // The minimal rate at exit
acceleration_steps_per_s2; // acceleration steps/sec^2
#if FAN_COUNT > 0
unsigned long fan_speed[FAN_COUNT];
@ -379,10 +391,10 @@ class Planner {
return sqrt(sq(target_velocity) - 2 * accel * distance);
}
static void calculate_trapezoid_for_block(block_t* block, float entry_factor, float exit_factor);
static void calculate_trapezoid_for_block(block_t* const block, const float &entry_factor, const float &exit_factor);
static void reverse_pass_kernel(block_t* current, block_t* next);
static void forward_pass_kernel(block_t* previous, block_t* current);
static void reverse_pass_kernel(block_t* const current, const block_t *next);
static void forward_pass_kernel(const block_t *previous, block_t* const current);
static void reverse_pass();
static void forward_pass();