andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Merge pull request #4997 from thinkyhead/rc_jerk_from_mk2 

Adapt Jerk / Speed code from Prusa MK2
pull/1/head
Scott Lahteine 8 years ago
committed by GitHub
parent
37643fef63 4d89652bc2
commit
0921c7da84
2 changed files with 201 additions and 116 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 253
      Marlin/planner.cpp
  2. 24
      Marlin/planner.h

253
Marlin/planner.cpp
View File

@ -85,7 +85,7 @@ float Planner::max_feedrate_mm_s[NUM_AXIS], // Max speeds in mm per second
Planner::axis_steps_per_mm[NUM_AXIS], Planner::axis_steps_per_mm[NUM_AXIS],
Planner::steps_to_mm[NUM_AXIS]; Planner::steps_to_mm[NUM_AXIS];
unsigned long Planner::max_acceleration_steps_per_s2[NUM_AXIS], uint32_t Planner::max_acceleration_steps_per_s2[NUM_AXIS],
Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software Planner::max_acceleration_mm_per_s2[NUM_AXIS]; // Use M201 to override by software
millis_t Planner::min_segment_time; millis_t Planner::min_segment_time;
@ -145,17 +145,19 @@ void Planner::init() {
#endif #endif
} }
#define MINIMAL_STEP_RATE 120
/** /**
* 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)
// Limit minimal step rate (Otherwise the timer will overflow.) // Limit minimal step rate (Otherwise the timer will overflow.)
NOLESS(initial_rate, 120); NOLESS(initial_rate, MINIMAL_STEP_RATE);
NOLESS(final_rate, 120); NOLESS(final_rate, MINIMAL_STEP_RATE);
int32_t accel = block->acceleration_steps_per_s2, int32_t accel = block->acceleration_steps_per_s2,
accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)), accelerate_steps = ceil(estimate_acceleration_distance(initial_rate, block->nominal_rate, accel)),
@ -172,13 +174,9 @@ void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor,
plateau_steps = 0; plateau_steps = 0;
} }
#if ENABLED(ADVANCE)
volatile int32_t initial_advance = block->advance * sq(entry_factor),
final_advance = block->advance * sq(exit_factor);
#endif // ADVANCE
// block->accelerate_until = accelerate_steps; // block->accelerate_until = accelerate_steps;
// block->decelerate_after = accelerate_steps+plateau_steps; // block->decelerate_after = accelerate_steps+plateau_steps;
CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section CRITICAL_SECTION_START; // Fill variables used by the stepper in a critical section
if (!block->busy) { // Don't update variables if block is busy. if (!block->busy) { // Don't update variables if block is busy.
block->accelerate_until = accelerate_steps; block->accelerate_until = accelerate_steps;
@ -186,8 +184,8 @@ void Planner::calculate_trapezoid_for_block(block_t* block, float entry_factor,
block->initial_rate = initial_rate; block->initial_rate = initial_rate;
block->final_rate = final_rate; block->final_rate = final_rate;
#if ENABLED(ADVANCE) #if ENABLED(ADVANCE)
block->initial_advance = initial_advance; block->initial_advance = block->advance * sq(entry_factor);
block->final_advance = final_advance; block->final_advance = block->advance * sq(exit_factor);
#endif #endif
} }
CRITICAL_SECTION_END; CRITICAL_SECTION_END;
@ -203,29 +201,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 (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; float max_entry_speed = current->max_entry_speed;
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.
if (!current->nominal_length_flag && max_entry_speed > next->entry_speed) { current->entry_speed = ((current->flag & BLOCK_FLAG_NOMINAL_LENGTH) || max_entry_speed <= next->entry_speed)
current->entry_speed = min(max_entry_speed, ? 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));
} current->flag |= BLOCK_FLAG_RECALCULATE;
else {
current->entry_speed = max_entry_speed;
}
current->recalculate_flag = true;
} }
} // Skip last block. Already initialized and set for recalculation.
} }
/** /**
@ -239,12 +228,14 @@ void Planner::reverse_pass() {
block_t* block[3] = { NULL, NULL, NULL }; block_t* block[3] = { NULL, NULL, NULL };
// Make a local copy of block_buffer_tail, because the interrupt can alter it // Make a local copy of block_buffer_tail, because the interrupt can alter it
CRITICAL_SECTION_START; // Is a critical section REALLY needed for a single byte change?
//CRITICAL_SECTION_START;
uint8_t tail = block_buffer_tail; uint8_t tail = block_buffer_tail;
CRITICAL_SECTION_END //CRITICAL_SECTION_END
uint8_t b = BLOCK_MOD(block_buffer_head - 3); uint8_t b = BLOCK_MOD(block_buffer_head - 3);
while (b != tail) { while (b != tail) {
if (block[0] && (block[0]->flag & BLOCK_FLAG_START_FROM_FULL_HALT)) break;
b = prev_block_index(b); b = prev_block_index(b);
block[2] = block[1]; block[2] = block[1];
block[1] = block[0]; block[1] = block[0];
@ -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;
} }
} }
@ -706,6 +696,9 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Bail if this is a zero-length block // Bail if this is a zero-length block
if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return; if (block->step_event_count < MIN_STEPS_PER_SEGMENT) return;
// Clear the block flags
block->flag = 0;
// For a mixing extruder, get a magnified step_event_count for each // For a mixing extruder, get a magnified step_event_count for each
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
for (uint8_t i = 0; i < MIXING_STEPPERS; i++) for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
@ -1021,51 +1014,67 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Compute and limit the acceleration rate for the trapezoid generator. // Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters; float steps_per_mm = block->step_event_count / block->millimeters;
uint32_t accel;
if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) { if (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS]) {
block->acceleration_steps_per_s2 = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2 // convert to: acceleration steps/sec^2
accel = ceil(retract_acceleration * steps_per_mm);
} }
else { else {
#define LIMIT_ACCEL(AXIS) do{ \
const uint32_t comp = max_acceleration_steps_per_s2[AXIS] * block->step_event_count; \
if (accel * block->steps[AXIS] > comp) accel = comp / block->steps[AXIS]; \
}while(0)
// Start with print or travel acceleration
accel = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm);
// Limit acceleration per axis // Limit acceleration per axis
block->acceleration_steps_per_s2 = ceil((block->steps[E_AXIS] ? acceleration : travel_acceleration) * steps_per_mm); LIMIT_ACCEL(X_AXIS);
if (max_acceleration_steps_per_s2[X_AXIS] < (block->acceleration_steps_per_s2 * block->steps[X_AXIS]) / block->step_event_count) LIMIT_ACCEL(Y_AXIS);
block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[X_AXIS] * block->step_event_count) / block->steps[X_AXIS]; LIMIT_ACCEL(Z_AXIS);
if (max_acceleration_steps_per_s2[Y_AXIS] < (block->acceleration_steps_per_s2 * block->steps[Y_AXIS]) / block->step_event_count) LIMIT_ACCEL(E_AXIS);
block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[Y_AXIS] * block->step_event_count) / block->steps[Y_AXIS]; }
if (max_acceleration_steps_per_s2[Z_AXIS] < (block->acceleration_steps_per_s2 * block->steps[Z_AXIS]) / block->step_event_count) block->acceleration_steps_per_s2 = accel;
block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[Z_AXIS] * block->step_event_count) / block->steps[Z_AXIS]; block->acceleration = accel / steps_per_mm;
if (max_acceleration_steps_per_s2[E_AXIS] < (block->acceleration_steps_per_s2 * block->steps[E_AXIS]) / block->step_event_count) block->acceleration_rate = (long)(accel * 16777216.0 / ((F_CPU) * 0.125)); // * 8.388608
block->acceleration_steps_per_s2 = (max_acceleration_steps_per_s2[E_AXIS] * block->step_event_count) / block->steps[E_AXIS];
} // Initial limit on the segment entry velocity
block->acceleration = block->acceleration_steps_per_s2 / steps_per_mm; float vmax_junction;
block->acceleration_rate = (long)(block->acceleration_steps_per_s2 * 16777216.0 / ((F_CPU) * 0.125));
#if 0 // Use old jerk for now #if 0 // Use old jerk for now
float junction_deviation = 0.1; float junction_deviation = 0.1;
// Compute path unit vector // Compute path unit vector
double unit_vec[XYZ]; double unit_vec[XYZ] = {
delta_mm[X_AXIS] * inverse_millimeters,
unit_vec[X_AXIS] = delta_mm[X_AXIS] * inverse_millimeters; delta_mm[Y_AXIS] * inverse_millimeters,
unit_vec[Y_AXIS] = delta_mm[Y_AXIS] * inverse_millimeters; delta_mm[Z_AXIS] * inverse_millimeters
unit_vec[Z_AXIS] = delta_mm[Z_AXIS] * inverse_millimeters; };
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation. /*
// Let a circle be tangent to both previous and current path line segments, where the junction Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// deviation is defined as the distance from the junction to the closest edge of the circle,
// collinear with the circle center. The circular segment joining the two paths represents the Let a circle be tangent to both previous and current path line segments, where the junction
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the deviation is defined as the distance from the junction to the closest edge of the circle,
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as collinear with the circle center.
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the The circular segment joining the two paths represents the path of centripetal acceleration.
// nonlinearities of both the junction angle and junction velocity. Solve for max velocity based on max acceleration about the radius of the circle, defined
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed indirectly by junction deviation.
This may be also viewed as path width or max_jerk in the previous grbl version. This approach
does not actually deviate from path, but used as a robust way to compute cornering speeds, as
it takes into account the nonlinearities of both the junction angle and junction velocity.
*/
vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles. // Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) { if (block_buffer_head != block_buffer_tail && previous_nominal_speed > 0.0) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative) // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity. // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS] float cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS] - previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ; - previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction. // Skip and use default max junction speed for 0 degree acute junction.
@ -1074,37 +1083,101 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds. // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) { if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation // Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive. float sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
NOMORE(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2))); NOMORE(vmax_junction, sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
} }
} }
} }
#endif #endif
// Start with a safe speed /**
float vmax_junction = max_jerk[X_AXIS] * 0.5, vmax_junction_factor = 1.0; * Adapted from Prusa MKS firmware
if (max_jerk[Y_AXIS] * 0.5 < fabs(current_speed[Y_AXIS])) NOMORE(vmax_junction, max_jerk[Y_AXIS] * 0.5); *
if (max_jerk[Z_AXIS] * 0.5 < fabs(current_speed[Z_AXIS])) NOMORE(vmax_junction, max_jerk[Z_AXIS] * 0.5); * Start with a safe speed (from which the machine may halt to stop immediately).
if (max_jerk[E_AXIS] * 0.5 < fabs(current_speed[E_AXIS])) NOMORE(vmax_junction, max_jerk[E_AXIS] * 0.5); */
NOMORE(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if (moves_queued > 1 && previous_nominal_speed > 0.0001) { // Exit speed limited by a jerk to full halt of a previous last segment
//if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) { static float previous_safe_speed;
vmax_junction = block->nominal_speed;
//}
float dsx = fabs(current_speed[X_AXIS] - previous_speed[X_AXIS]), float safe_speed = block->nominal_speed;
dsy = fabs(current_speed[Y_AXIS] - previous_speed[Y_AXIS]), bool limited = false;
dsz = fabs(current_speed[Z_AXIS] - previous_speed[Z_AXIS]), LOOP_XYZE(i) {
dse = fabs(current_speed[E_AXIS] - previous_speed[E_AXIS]); float jerk = fabs(current_speed[i]);
if (dsx > max_jerk[X_AXIS]) NOMORE(vmax_junction_factor, max_jerk[X_AXIS] / dsx); if (jerk > max_jerk[i]) {
if (dsy > max_jerk[Y_AXIS]) NOMORE(vmax_junction_factor, max_jerk[Y_AXIS] / dsy); // The actual jerk is lower if it has been limited by the XY jerk.
if (dsz > max_jerk[Z_AXIS]) NOMORE(vmax_junction_factor, max_jerk[Z_AXIS] / dsz); if (limited) {
if (dse > max_jerk[E_AXIS]) NOMORE(vmax_junction_factor, max_jerk[E_AXIS] / dse); // Spare one division by a following gymnastics:
// Instead of jerk *= safe_speed / block->nominal_speed,
// multiply max_jerk[i] by the divisor.
jerk *= safe_speed;
float mjerk = max_jerk[i] * block->nominal_speed;
if (jerk > mjerk) safe_speed *= mjerk / jerk;
}
else {
safe_speed = max_jerk[i];
limited = true;
}
}
}
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed if (moves_queued > 1 && previous_nominal_speed > 0.0001) {
// Estimate a maximum velocity allowed at a joint of two successive segments.
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
// then the machine is not coasting anymore and the safe entry / exit velocities shall be used.
// The junction velocity will be shared between successive segments. Limit the junction velocity to their minimum.
bool prev_speed_larger = previous_nominal_speed > block->nominal_speed;
float smaller_speed_factor = prev_speed_larger ? (block->nominal_speed / previous_nominal_speed) : (previous_nominal_speed / block->nominal_speed);
// Pick the smaller of the nominal speeds. Higher speed shall not be achieved at the junction during coasting.
vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed;
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
float v_factor = 1.f;
limited = false;
// Now limit the jerk in all axes.
LOOP_XYZE(axis) {
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.
float v_exit = previous_speed[axis], v_entry = current_speed[axis];
if (prev_speed_larger) v_exit *= smaller_speed_factor;
if (limited) {
v_exit *= v_factor;
v_entry *= v_factor;
}
// Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
float jerk =
(v_exit > v_entry) ?
((v_entry > 0.f || v_exit < 0.f) ?
// coasting
(v_exit - v_entry) :
// axis reversal
max(v_exit, -v_entry)) :
// v_exit <= v_entry
((v_entry < 0.f || v_exit > 0.f) ?
// coasting
(v_entry - v_exit) :
// axis reversal
max(-v_exit, v_entry));
if (jerk > max_jerk[axis]) {
v_factor *= max_jerk[axis] / jerk;
limited = true;
}
}
if (limited) vmax_junction *= v_factor;
// Now the transition velocity is known, which maximizes the shared exit / entry velocity while
// respecting the jerk factors, it may be possible, that applying separate safe exit / entry velocities will achieve faster prints.
float vmax_junction_threshold = vmax_junction * 0.99f;
if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) {
// Not coasting. The machine will stop and start the movements anyway,
// better to start the segment from start.
block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
vmax_junction = safe_speed;
}
} }
else {
block->flag |= BLOCK_FLAG_START_FROM_FULL_HALT;
vmax_junction = safe_speed;
}
// Max entry speed of this block equals the max exit speed of the previous block.
block->max_entry_speed = vmax_junction; block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED. // Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
@ -1119,12 +1192,12 @@ 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_RECALCULATE | (block->nominal_speed <= v_allowable ? BLOCK_FLAG_NOMINAL_LENGTH : 0);
block->recalculate_flag = true; // 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));
previous_nominal_speed = block->nominal_speed; previous_nominal_speed = block->nominal_speed;
previous_safe_speed = safe_speed;
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)

24
Marlin/planner.h
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
* *
@ -84,11 +97,10 @@ typedef struct {
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
@ -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();

Write Preview
Loading…
Cancel
Save