MPCNC/Firmware2
1
0
Fork 0
Code Issues 1 Pull Requests Releases Wiki Activity

Improved Core-compatible jerk code

Browse Source
This commit is contained in:
Scott Lahteine 2017-12-06 23:57:57 -06:00
parent 81c0a918af
commit 3cd7659924
1 changed files with 11 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
Marlin/src/module/planner.cpp
View File

@ -1090,10 +1090,13 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
}
#endif
// Calculate and limit speed in mm/sec for each axis
// Calculate and limit speed in mm/sec for each axis, calculate minimum acceleration ratio
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) {
const float cs = FABS((current_speed[i] = delta_mm[i] * inverse_secs));
NOMORE(min_axis_accel_ratio, max_jerk[i] / cs);
NOLESS(max_stepper_speed, cs);
#if ENABLED(DISTINCT_E_FACTORS)
if (i == E_AXIS) i += extruder;
#endif
@ -1138,6 +1141,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
}
#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
if (speed_factor < 1.0) {
LOOP_XYZE(i) current_speed[i] *= speed_factor;
@ -1145,6 +1151,9 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
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.
const float steps_per_mm = block->step_event_count * inverse_millimeters;
uint32_t accel;
@ -1246,32 +1255,6 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const
}
#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)) {
// 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,
@ -1283,7 +1266,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.
float v_factor = 1;
limited = 0;
uint8_t limited = 0;
// Now limit the jerk in all axes.
const float smaller_speed_factor = vmax_junction / previous_nominal_speed;