parent
5bc2acc072
commit
97d509d4d2
|
@ -1110,14 +1110,10 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// Calculate and limit speed in mm/sec for each axis, calculate minimum acceleration ratio
|
// Calculate and limit speed in mm/sec for each axis
|
||||||
float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
|
float current_speed[NUM_AXIS], speed_factor = 1.0; // factor <1 decreases speed
|
||||||
float max_stepper_speed = 0, min_axis_accel_ratio = 1; // ratio < 1 means acceleration ramp needed
|
|
||||||
LOOP_XYZE(i) {
|
LOOP_XYZE(i) {
|
||||||
const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_secs));
|
const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_secs));
|
||||||
if (cs > max_jerk[i])
|
|
||||||
NOMORE(min_axis_accel_ratio, max_jerk[i] / cs);
|
|
||||||
NOLESS(max_stepper_speed, cs);
|
|
||||||
#if ENABLED(DISTINCT_E_FACTORS)
|
#if ENABLED(DISTINCT_E_FACTORS)
|
||||||
if (i == E_AXIS) i += extruder;
|
if (i == E_AXIS) i += extruder;
|
||||||
#endif
|
#endif
|
||||||
|
@ -1162,9 +1158,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
||||||
}
|
}
|
||||||
#endif // XY_FREQUENCY_LIMIT
|
#endif // XY_FREQUENCY_LIMIT
|
||||||
|
|
||||||
block->nominal_speed = max_stepper_speed; // (mm/sec) Always > 0
|
|
||||||
block->nominal_rate = CEIL(block->step_event_count * inverse_secs); // (step/sec) Always > 0
|
|
||||||
|
|
||||||
// Correct the speed
|
// Correct the speed
|
||||||
if (speed_factor < 1.0) {
|
if (speed_factor < 1.0) {
|
||||||
LOOP_XYZE(i) current_speed[i] *= speed_factor;
|
LOOP_XYZE(i) current_speed[i] *= speed_factor;
|
||||||
|
@ -1172,9 +1165,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
||||||
block->nominal_rate *= speed_factor;
|
block->nominal_rate *= speed_factor;
|
||||||
}
|
}
|
||||||
|
|
||||||
float safe_speed = block->nominal_speed * min_axis_accel_ratio;
|
|
||||||
static float previous_safe_speed;
|
|
||||||
|
|
||||||
// Compute and limit the acceleration rate for the trapezoid generator.
|
// Compute and limit the acceleration rate for the trapezoid generator.
|
||||||
const float steps_per_mm = block->step_event_count * inverse_millimeters;
|
const float steps_per_mm = block->step_event_count * inverse_millimeters;
|
||||||
uint32_t accel;
|
uint32_t accel;
|
||||||
|
@ -1276,6 +1266,32 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Adapted from Průša MKS firmware
|
||||||
|
* https://github.com/prusa3d/Prusa-Firmware
|
||||||
|
*
|
||||||
|
* Start with a safe speed (from which the machine may halt to stop immediately).
|
||||||
|
*/
|
||||||
|
|
||||||
|
// Exit speed limited by a jerk to full halt of a previous last segment
|
||||||
|
static float previous_safe_speed;
|
||||||
|
|
||||||
|
float safe_speed = block->nominal_speed;
|
||||||
|
uint8_t limited = 0;
|
||||||
|
LOOP_XYZE(i) {
|
||||||
|
const float jerk = FABS(current_speed[i]), maxj = max_jerk[i];
|
||||||
|
if (jerk > maxj) {
|
||||||
|
if (limited) {
|
||||||
|
const float mjerk = maxj * block->nominal_speed;
|
||||||
|
if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
++limited;
|
||||||
|
safe_speed = maxj;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
|
if (moves_queued && !UNEAR_ZERO(previous_nominal_speed)) {
|
||||||
// Estimate a maximum velocity allowed at a joint of two successive segments.
|
// 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,
|
// If this maximum velocity allowed is lower than the minimum of the entry / exit safe velocities,
|
||||||
|
@ -1287,7 +1303,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
|
||||||
|
|
||||||
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
|
// Factor to multiply the previous / current nominal velocities to get componentwise limited velocities.
|
||||||
float v_factor = 1;
|
float v_factor = 1;
|
||||||
uint8_t limited = 0;
|
limited = 0;
|
||||||
|
|
||||||
// Now limit the jerk in all axes.
|
// Now limit the jerk in all axes.
|
||||||
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
|
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;
|
||||||
|
|
Loading…
Reference in a new issue