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 * Calculate trapezoid parameters, multiplying the entry- and exit-speeds
* by the provided factors. * 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), uint32_t initial_rate = ceil(block->nominal_rate * entry_factor),
final_rate = ceil(block->nominal_rate * exit_factor); // (steps per second) 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. // 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) { void Planner::reverse_pass_kernel(block_t* const current, const block_t *next) {
if (!current) return; if (!current || !next) return;
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
if (next) { // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
// If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising. // check for maximum allowable speed reductions to ensure maximum possible planned speed.
// If not, block in state of acceleration or deceleration. Reset entry speed to maximum and float max_entry_speed = current->max_entry_speed;
// check for maximum allowable speed reductions to ensure maximum possible planned speed. if (current->entry_speed != max_entry_speed) {
float max_entry_speed = current->max_entry_speed; // If nominal length true, max junction speed is guaranteed to be reached. Only compute
if (current->entry_speed != max_entry_speed) { // 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)
// If nominal length true, max junction speed is guaranteed to be reached. Only compute ? max_entry_speed
// for max allowable speed if block is decelerating and nominal length is false. : min(max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->millimeters));
if (!current->nominal_length_flag && max_entry_speed > next->entry_speed) { current->flag |= BLOCK_FLAG_RECALCULATE;
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.
} }
/** /**
@ -255,21 +246,21 @@ 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(block_t* previous, block_t* current) { void Planner::forward_pass_kernel(const block_t* previous, block_t* const current) {
if (!previous) return; if (!previous) return;
// If the previous block is an acceleration block, but it is not long enough to complete the // 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 // 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. // 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 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) { if (previous->entry_speed < current->entry_speed) {
float entry_speed = min(current->entry_speed, float entry_speed = min(current->entry_speed,
max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters)); max_allowable_speed(-previous->acceleration, previous->entry_speed, previous->millimeters));
// Check for junction speed change // Check for junction speed change
if (current->entry_speed != entry_speed) { if (current->entry_speed != entry_speed) {
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() { void Planner::recalculate_trapezoids() {
int8_t block_index = block_buffer_tail; int8_t block_index = block_buffer_tail;
block_t* current; block_t *current, *next = NULL;
block_t* next = NULL;
while (block_index != block_buffer_head) { while (block_index != block_buffer_head) {
current = next; current = next;
next = &block_buffer[block_index]; next = &block_buffer[block_index];
if (current) { if (current) {
// Recalculate if current block entry or exit junction speed has changed. // 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. // NOTE: Entry and exit factors always > 0 by all previous logic operations.
float nom = current->nominal_speed; float nom = current->nominal_speed;
calculate_trapezoid_for_block(current, current->entry_speed / nom, next->entry_speed / nom); 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); block_index = next_block_index(block_index);
@ -319,7 +309,7 @@ void Planner::recalculate_trapezoids() {
if (next) { if (next) {
float nom = next->nominal_speed; float nom = next->nominal_speed;
calculate_trapezoid_for_block(next, next->entry_speed / nom, (MINIMUM_PLANNER_SPEED) / nom); 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 // 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 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. // the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable); block->flag &= ~BLOCK_FLAG_NOMINAL_LENGTH;
block->recalculate_flag = true; // Always calculate trapezoid for new block 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 // Update previous path unit_vector and nominal speed
memcpy(previous_speed, current_speed, sizeof(previous_speed)); memcpy(previous_speed, current_speed, sizeof(previous_speed));

View file

@ -40,6 +40,19 @@
#include "vector_3.h" #include "vector_3.h"
#endif #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 * struct block_t
* *
@ -79,19 +92,18 @@ typedef struct {
#endif #endif
// Fields used by the motion planner to manage acceleration // Fields used by the motion planner to manage acceleration
float nominal_speed, // The nominal speed for this block in mm/sec float nominal_speed, // The nominal speed for this block in mm/sec
entry_speed, // Entry speed at previous-current junction 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 max_entry_speed, // Maximum allowable junction entry speed in mm/sec
millimeters, // The total travel of this block in mm millimeters, // The total travel of this block in mm
acceleration; // acceleration mm/sec^2 acceleration; // acceleration mm/sec^2
unsigned char recalculate_flag, // Planner flag to recalculate trapezoids on entry junction uint8_t flag; // Block flags (See BlockFlag enum above)
nominal_length_flag; // Planner flag for nominal speed always reached
// Settings for the trapezoid generator // Settings for the trapezoid generator
unsigned long nominal_rate, // The nominal step rate for this block in step_events/sec 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 initial_rate, // The jerk-adjusted step rate at start of block
final_rate, // The minimal rate at exit final_rate, // The minimal rate at exit
acceleration_steps_per_s2; // acceleration steps/sec^2 acceleration_steps_per_s2; // acceleration steps/sec^2
#if FAN_COUNT > 0 #if FAN_COUNT > 0
unsigned long fan_speed[FAN_COUNT]; unsigned long fan_speed[FAN_COUNT];
@ -379,10 +391,10 @@ class Planner {
return sqrt(sq(target_velocity) - 2 * accel * distance); 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 reverse_pass_kernel(block_t* const current, const block_t *next);
static void forward_pass_kernel(block_t* previous, block_t* current); static void forward_pass_kernel(const block_t *previous, block_t* const current);
static void reverse_pass(); static void reverse_pass();
static void forward_pass(); static void forward_pass();