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

Merge pull request #5445 from thinkyhead/rc_reduce_jerk_code

Browse Source 
Slightly shrink jerk and advance code
This commit is contained in:
Scott Lahteine 2016-12-09 02:54:03 -08:00 committed by GitHub
commit 8383f35b40
1 changed files with 66 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

139
Marlin/planner.cpp
View File

@ -141,8 +141,8 @@ float Planner::previous_speed[NUM_AXIS],
#endif #endif
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
float Planner::extruder_advance_k = LIN_ADVANCE_K; float Planner::extruder_advance_k = LIN_ADVANCE_K,
float Planner::position_float[NUM_AXIS] = { 0 }; Planner::position_float[NUM_AXIS] = { 0 };
#endif #endif
#if ENABLED(ENSURE_SMOOTH_MOVES) #if ENABLED(ENSURE_SMOOTH_MOVES)
@ -654,7 +654,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// The target position of the tool in absolute steps // The target position of the tool in absolute steps
// Calculate target position in absolute steps // Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[XYZE] = { const long target[XYZE] = {
lround(a * axis_steps_per_mm[X_AXIS]), lround(a * axis_steps_per_mm[X_AXIS]),
lround(b * axis_steps_per_mm[Y_AXIS]), lround(b * axis_steps_per_mm[Y_AXIS]),
lround(c * axis_steps_per_mm[Z_AXIS]), lround(c * axis_steps_per_mm[Z_AXIS]),
@ -670,16 +670,16 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
#endif #endif
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
float target_float[XYZE] = {a, b, c, e}; const float target_float[XYZE] = { a, b, c, e },
float de_float = target_float[E_AXIS] - position_float[E_AXIS]; de_float = target_float[E_AXIS] - position_float[E_AXIS],
float mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS])); mm_D_float = sqrt(sq(target_float[X_AXIS] - position_float[X_AXIS]) + sq(target_float[Y_AXIS] - position_float[Y_AXIS]));
memcpy(position_float, target_float, sizeof(position_float)); memcpy(position_float, target_float, sizeof(position_float));
#endif #endif
long da = target[X_AXIS] - position[X_AXIS], const long da = target[X_AXIS] - position[X_AXIS],
db = target[Y_AXIS] - position[Y_AXIS], db = target[Y_AXIS] - position[Y_AXIS],
dc = target[Z_AXIS] - position[Z_AXIS]; dc = target[Z_AXIS] - position[Z_AXIS];
/* /*
SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s); SERIAL_ECHOPAIR(" Planner FR:", fr_mm_s);
@ -755,11 +755,11 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
#endif #endif
if (de < 0) SBI(dm, E_AXIS); if (de < 0) SBI(dm, E_AXIS);
float esteps_float = de * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01; const float esteps_float = de * volumetric_multiplier[extruder] * flow_percentage[extruder] * 0.01;
int32_t esteps = abs(esteps_float) + 0.5; const int32_t esteps = abs(esteps_float) + 0.5;
// Calculate the buffer head after we push this byte // Calculate the buffer head after we push this byte
int8_t next_buffer_head = next_block_index(block_buffer_head); const uint8_t next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot. // If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer. // Rest here until there is room in the buffer.
@ -852,7 +852,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
#if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder #if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder
for (int8_t i = 0; i < EXTRUDERS; i++) for (uint8_t i = 0; i < EXTRUDERS; i++)
if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--; if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
switch(extruder) { switch(extruder) {
@ -980,7 +980,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Calculate moves/second for this move. No divide by zero due to previous checks. // Calculate moves/second for this move. No divide by zero due to previous checks.
float inverse_mm_s = fr_mm_s * inverse_millimeters; float inverse_mm_s = fr_mm_s * inverse_millimeters;
int moves_queued = movesplanned(); const uint8_t moves_queued = movesplanned();
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill // Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
#if ENABLED(SLOWDOWN) #if ENABLED(SLOWDOWN)
@ -1037,7 +1037,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// If the index has changed (must have gone forward)... // If the index has changed (must have gone forward)...
if (filwidth_delay_index[0] != filwidth_delay_index[1]) { if (filwidth_delay_index[0] != filwidth_delay_index[1]) {
filwidth_e_count = 0; // Reset the E movement counter filwidth_e_count = 0; // Reset the E movement counter
int8_t meas_sample = thermalManager.widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char const int8_t meas_sample = thermalManager.widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char
do { do {
filwidth_delay_index[1] = (filwidth_delay_index[1] + 1) % MMD_CM; // The next unused slot filwidth_delay_index[1] = (filwidth_delay_index[1] + 1) % MMD_CM; // The next unused slot
measurement_delay[filwidth_delay_index[1]] = meas_sample; // Store the measurement measurement_delay[filwidth_delay_index[1]] = meas_sample; // Store the measurement
@ -1050,7 +1050,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
// Calculate and limit speed in mm/sec for each axis // 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
LOOP_XYZE(i) { LOOP_XYZE(i) {
float cs = fabs(current_speed[i] = delta_mm[i] * inverse_mm_s); const float cs = fabs(current_speed[i] = delta_mm[i] * inverse_mm_s);
if (cs > max_feedrate_mm_s[i]) NOMORE(speed_factor, max_feedrate_mm_s[i] / cs); if (cs > max_feedrate_mm_s[i]) NOMORE(speed_factor, max_feedrate_mm_s[i] / cs);
} }
@ -1058,7 +1058,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
#ifdef XY_FREQUENCY_LIMIT #ifdef XY_FREQUENCY_LIMIT
// Check and limit the xy direction change frequency // Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits; const unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits; old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor); segment_time = lround((float)segment_time / speed_factor);
@ -1083,11 +1083,11 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
} }
ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time; ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;
long max_x_segment_time = MAX3(xs0, xs1, xs2), const long max_x_segment_time = MAX3(xs0, xs1, xs2),
max_y_segment_time = MAX3(ys0, ys1, ys2), max_y_segment_time = MAX3(ys0, ys1, ys2),
min_xy_segment_time = min(max_x_segment_time, max_y_segment_time); min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
if (min_xy_segment_time < MAX_FREQ_TIME) { if (min_xy_segment_time < MAX_FREQ_TIME) {
float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME); const float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME);
NOMORE(speed_factor, low_sf); NOMORE(speed_factor, low_sf);
} }
#endif // XY_FREQUENCY_LIMIT #endif // XY_FREQUENCY_LIMIT
@ -1100,7 +1100,7 @@ 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 * inverse_millimeters; const float steps_per_mm = block->step_event_count * inverse_millimeters;
uint32_t accel; 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]) {
// convert to: acceleration steps/sec^2 // convert to: acceleration steps/sec^2
@ -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.
@ -1247,28 +1242,21 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
v_entry *= v_factor; v_entry *= v_factor;
} }
// Calculate jerk depending on whether the axis is coasting in the same direction or reversing. // Calculate jerk depending on whether the axis is coasting in the same direction or reversing.
float jerk = const float jerk = (v_exit > v_entry)
(v_exit > v_entry) ? ? // coasting axis reversal
((v_entry > 0.f || v_exit < 0.f) ? ( (v_entry > 0.f || v_exit < 0.f) ? (v_exit - v_entry) : max(v_exit, -v_entry) )
// coasting : // v_exit <= v_entry coasting axis reversal
(v_exit - v_entry) : ( (v_entry < 0.f || v_exit > 0.f) ? (v_entry - v_exit) : max(-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]) { if (jerk > max_jerk[axis]) {
v_factor *= max_jerk[axis] / jerk; v_factor *= max_jerk[axis] / jerk;
limited = true; ++limited;
} }
} }
if (limited) vmax_junction *= v_factor; if (limited) vmax_junction *= v_factor;
// Now the transition velocity is known, which maximizes the shared exit / entry velocity while // 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. // 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; const float vmax_junction_threshold = vmax_junction * 0.99f;
if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) { if (previous_safe_speed > vmax_junction_threshold && safe_speed > vmax_junction_threshold) {
// Not coasting. The machine will stop and start the movements anyway, // Not coasting. The machine will stop and start the movements anyway,
// better to start the segment from start. // better to start the segment from start.
@ -1285,7 +1273,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
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.
float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters); const float v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable); block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags // Initialize planner efficiency flags
@ -1305,36 +1293,41 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const
#if ENABLED(LIN_ADVANCE) #if ENABLED(LIN_ADVANCE)
// Don't use LIN_ADVANCE for blocks if: //
// !block->steps[E_AXIS]: We don't have E steps todo (Travel move) // Use LIN_ADVANCE for blocks if all these are true:
// !block->steps[X_AXIS] && !block->steps[Y_AXIS]: We don't have a movement in XY direction (Retract / Prime moves) //
// extruder_advance_k == 0.0: There is no advance factor set // esteps : We have E steps todo (a printing move)
// block->steps[E_AXIS] == block->step_event_count: A problem occurs when there's a very tiny move before a retract. //
// In this case, the retract and the move will be executed together. // block->steps[X_AXIS] || block->steps[Y_AXIS] : We have a movement in XY direction (i.e., not retract / prime).
// This leads to an enormous number of advance steps due to a huge e_acceleration. //
// The math is correct, but you don't want a retract move done with advance! // extruder_advance_k : There is an advance factor set.
// de_float <= 0.0: Extruder is running in reverse direction (for example during "Wipe while retracting" (Slic3r) or "Combing" (Cura) movements) //
if (!esteps || (!block->steps[X_AXIS] && !block->steps[Y_AXIS]) || extruder_advance_k == 0.0 || (uint32_t)esteps == block->step_event_count || de_float <= 0.0) { // block->steps[E_AXIS] != block->step_event_count : A problem occurs if the move before a retract is too small.
block->use_advance_lead = false; // In that case, the retract and move will be executed together.
} // This leads to too many advance steps due to a huge e_acceleration.
else { // The math is good, but we must avoid retract moves with advance!
block->use_advance_lead = true; // de_float > 0.0 : Extruder is running forward (e.g., for "Wipe while retracting" (Slic3r) or "Combing" (Cura) moves)
//
block->use_advance_lead = esteps
&& (block->steps[X_AXIS] || block->steps[Y_AXIS])
&& extruder_advance_k
&& (uint32_t)esteps != block->step_event_count
&& de_float > 0.0;
if (block->use_advance_lead)
block->abs_adv_steps_multiplier8 = lround(extruder_advance_k * (de_float / mm_D_float) * block->nominal_speed / (float)block->nominal_rate * axis_steps_per_mm[E_AXIS_N] * 256.0); block->abs_adv_steps_multiplier8 = lround(extruder_advance_k * (de_float / mm_D_float) * block->nominal_speed / (float)block->nominal_rate * axis_steps_per_mm[E_AXIS_N] * 256.0);
}
#elif ENABLED(ADVANCE) #elif ENABLED(ADVANCE)
// Calculate advance rate // Calculate advance rate
if (!esteps || (!block->steps[X_AXIS] && !block->steps[Y_AXIS] && !block->steps[Z_AXIS])) { if (esteps && (block->steps[X_AXIS] || block->steps[Y_AXIS] || block->steps[Z_AXIS])) {
block->advance_rate = 0; const long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_steps_per_s2);
block->advance = 0; const float advance = ((STEPS_PER_CUBIC_MM_E) * (EXTRUDER_ADVANCE_K)) * HYPOT(current_speed[E_AXIS], EXTRUSION_AREA) * 256;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_steps_per_s2);
float advance = ((STEPS_PER_CUBIC_MM_E) * (EXTRUDER_ADVANCE_K)) * HYPOT(current_speed[E_AXIS], EXTRUSION_AREA) * 256;
block->advance = advance; block->advance = advance;
block->advance_rate = acc_dist ? advance / (float)acc_dist : 0; block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
} }
else
block->advance_rate = block->advance = 0;
/** /**
SERIAL_ECHO_START; SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :"); SERIAL_ECHOPGM("advance :");