Slightly shrink code for jerk-based reduction of safe_speed

This commit is contained in:
Scott Lahteine 2016-12-08 22:24:29 -08:00
parent dfdb180db4
commit 3f3b5c86d8

View File

@ -1204,22 +1204,17 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
static float previous_safe_speed; static float previous_safe_speed;
float safe_speed = block->nominal_speed; float safe_speed = block->nominal_speed;
bool limited = false; uint8_t limited = 0;
LOOP_XYZE(i) { LOOP_XYZE(i) {
float jerk = fabs(current_speed[i]); const float jerk = fabs(current_speed[i]), maxj = max_jerk[i];
if (jerk > max_jerk[i]) { if (jerk > maxj) {
// The actual jerk is lower if it has been limited by the XY jerk.
if (limited) { if (limited) {
// Spare one division by a following gymnastics: const float mjerk = maxj * block->nominal_speed;
// Instead of jerk *= safe_speed / block->nominal_speed, if (jerk * safe_speed > mjerk) safe_speed = mjerk / jerk;
// 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 { else {
safe_speed = max_jerk[i]; ++limited;
limited = true; safe_speed = maxj;
} }
} }
} }
@ -1236,7 +1231,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed; vmax_junction = prev_speed_larger ? block->nominal_speed : previous_nominal_speed;
// 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.f; float v_factor = 1.f;
limited = false; limited = 0;
// Now limit the jerk in all axes. // Now limit the jerk in all axes.
LOOP_XYZE(axis) { LOOP_XYZE(axis) {
// Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop. // Limit an axis. We have to differentiate: coasting, reversal of an axis, full stop.