zaubentrucker/muele-marlin
1
0
Fork 0
Code Issues Pull requests Actions Packages Projects Releases Wiki Activity

🐛 Fix G2/G3 Arcs stutter / JD speed (#24362)

Browse source
This commit is contained in:
tombrazier 2022-06-18 05:17:12 +01:00 committed by Scott Lahteine
parent ae8365a4e5
commit 1efe48ef65
1 changed files with 93 additions and 94 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

187
Marlin/src/gcode/motion/G2_G3.cpp
View file

@ -179,8 +179,8 @@ void plan_arc(
// Feedrate for the move, scaled by the feedrate multiplier // Feedrate for the move, scaled by the feedrate multiplier
const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s); const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
// Get the nominal segment length based on settings // Get the ideal segment length for the move based on settings
const float nominal_segment_mm = ( const float ideal_segment_mm = (
#if ARC_SEGMENTS_PER_SEC // Length based on segments per second and feedrate #if ARC_SEGMENTS_PER_SEC // Length based on segments per second and feedrate
constrain(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM) constrain(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM)
#else #else
@ -188,19 +188,18 @@ void plan_arc(
#endif #endif
); );
// Number of whole segments based on the nominal segment length // Number of whole segments based on the ideal segment length
const float nominal_segments = _MAX(FLOOR(flat_mm / nominal_segment_mm), min_segments); const float nominal_segments = _MAX(FLOOR(flat_mm / ideal_segment_mm), min_segments),
nominal_segment_mm = flat_mm / nominal_segments;
// A new segment length based on the required minimum // The number of whole segments in the arc, with best attempt to honor MIN_ARC_SEGMENT_MM and MAX_ARC_SEGMENT_MM
const float segment_mm = constrain(flat_mm / nominal_segments, MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM); const uint16_t segments = nominal_segment_mm > (MAX_ARC_SEGMENT_MM) ? CEIL(flat_mm / (MAX_ARC_SEGMENT_MM)) :
nominal_segment_mm < (MIN_ARC_SEGMENT_MM) ? _MAX(1, FLOOR(flat_mm / (MIN_ARC_SEGMENT_MM))) :
nominal_segments;
// The number of whole segments in the arc, ignoring the remainder #if ENABLED(SCARA_FEEDRATE_SCALING)
uint16_t segments = FLOOR(flat_mm / segment_mm); const float inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
#endif
// Are the segments now too few to reach the destination?
const float segmented_length = segment_mm * segments;
const bool tooshort = segmented_length < flat_mm - 0.0001f;
const float proportion = tooshort ? segmented_length / flat_mm : 1.0f;
/** /**
* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
@ -228,98 +227,98 @@ void plan_arc(
* a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead. * a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
* This is important when there are successive arc motions. * This is important when there are successive arc motions.
*/ */
// Vector rotation matrix values
xyze_pos_t raw; xyze_pos_t raw;
const float theta_per_segment = proportion * angular_travel / segments,
sq_theta_per_segment = sq(theta_per_segment),
sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
#if DISABLED(AUTO_BED_LEVELING_UBL) // do not calculate rotation parameters for trivial single-segment arcs
ARC_LIJK_CODE( if (segments > 1) {
const float per_segment_L = proportion * travel_L / segments, // Vector rotation matrix values
const float per_segment_I = proportion * travel_I / segments, const float theta_per_segment = angular_travel / segments,
const float per_segment_J = proportion * travel_J / segments, sq_theta_per_segment = sq(theta_per_segment),
const float per_segment_K = proportion * travel_K / segments sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
#if DISABLED(AUTO_BED_LEVELING_UBL)
ARC_LIJK_CODE(
const float per_segment_L = travel_L / segments,
const float per_segment_I = travel_I / segments,
const float per_segment_J = travel_J / segments,
const float per_segment_K = travel_K / segments
);
#endif
CODE_ITEM_E(const float extruder_per_segment = travel_E / segments);
// Initialize all linear axes and E
ARC_LIJKE_CODE(
raw[axis_l] = current_position[axis_l],
raw.i = current_position.i,
raw.j = current_position.j,
raw.k = current_position.k,
raw.e = current_position.e
); );
#endif
CODE_ITEM_E(const float extruder_per_segment = proportion * travel_E / segments); millis_t next_idle_ms = millis() + 200UL;
// For shortened segments, run all but the remainder in the loop
if (tooshort) segments++;
// Initialize all linear axes and E
ARC_LIJKE_CODE(
raw[axis_l] = current_position[axis_l],
raw.i = current_position.i,
raw.j = current_position.j,
raw.k = current_position.k,
raw.e = current_position.e
);
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / segment_mm;
#endif
millis_t next_idle_ms = millis() + 200UL;
#if N_ARC_CORRECTION > 1
int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
thermalManager.manage_heater();
const millis_t ms = millis();
if (ELAPSED(ms, next_idle_ms)) {
next_idle_ms = ms + 200UL;
idle();
}
#if N_ARC_CORRECTION > 1 #if N_ARC_CORRECTION > 1
if (--arc_recalc_count) { int8_t arc_recalc_count = N_ARC_CORRECTION;
// Apply vector rotation matrix to previous rvec.a / 1 #endif
const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
rvec.a = rvec.a * cos_T - rvec.b * sin_T; for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
rvec.b = r_new_Y;
thermalManager.manage_heater();
const millis_t ms = millis();
if (ELAPSED(ms, next_idle_ms)) {
next_idle_ms = ms + 200UL;
idle();
} }
else
#endif
{
#if N_ARC_CORRECTION > 1 #if N_ARC_CORRECTION > 1
arc_recalc_count = N_ARC_CORRECTION; if (--arc_recalc_count) {
// Apply vector rotation matrix to previous rvec.a / 1
const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
rvec.a = rvec.a * cos_T - rvec.b * sin_T;
rvec.b = r_new_Y;
}
else
#endif
{
#if N_ARC_CORRECTION > 1
arc_recalc_count = N_ARC_CORRECTION;
#endif
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
// Compute exact location by applying transformation matrix from initial radius vector(=-offset).
// To reduce stuttering, the sin and cos could be computed at different times.
// For now, compute both at the same time.
const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
}
// Update raw location
raw[axis_p] = center_P + rvec.a;
raw[axis_q] = center_Q + rvec.b;
ARC_LIJKE_CODE(
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[axis_l] = start_L,
raw.i = start_I, raw.j = start_J, raw.k = start_K
#else
raw[axis_l] += per_segment_L,
raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K
#endif
, raw.e += extruder_per_segment
);
apply_motion_limits(raw);
#if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw);
#endif #endif
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
// Compute exact location by applying transformation matrix from initial radius vector(=-offset). break;
// To reduce stuttering, the sin and cos could be computed at different times.
// For now, compute both at the same time.
const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
} }
// Update raw location
raw[axis_p] = center_P + rvec.a;
raw[axis_q] = center_Q + rvec.b;
ARC_LIJKE_CODE(
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[axis_l] = start_L, raw.i = start_I, raw.j = start_J, raw.k = start_K
#else
raw[axis_l] += per_segment_L, raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K
#endif
, raw.e += extruder_per_segment
);
apply_motion_limits(raw);
#if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw);
#endif
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
break;
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.