andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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

FB4S_WIFI
tombrazier 2 years ago
committed by Scott Lahteine
parent
106537ff43
commit
0523874e9c
1 changed files with 97 additions and 100 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 197
      Marlin/src/gcode/motion/G2_G3.cpp

197
Marlin/src/gcode/motion/G2_G3.cpp
View File

@ -197,8 +197,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
@ -206,19 +206,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,
@ -246,108 +245,106 @@ 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;
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)
ARC_LIJKUVW_CODE(
const float per_segment_L = proportion * travel_L / segments,
const float per_segment_I = proportion * travel_I / segments,
const float per_segment_J = proportion * travel_J / segments,
const float per_segment_K = proportion * travel_K / segments,
const float per_segment_U = proportion * travel_U / segments,
const float per_segment_V = proportion * travel_V / segments,
const float per_segment_W = proportion * travel_W / segments
);
#endif
CODE_ITEM_E(const float extruder_per_segment = proportion * travel_E / segments); xyze_pos_t raw;
// For shortened segments, run all but the remainder in the loop
if (tooshort) segments++;
// Initialize all linear axes and E // do not calculate rotation parameters for trivial single-segment arcs
ARC_LIJKUVWE_CODE( if (segments > 1) {
raw[axis_l] = current_position[axis_l], // Vector rotation matrix values
raw.i = current_position.i, const float theta_per_segment = angular_travel / segments,
raw.j = current_position.j, sq_theta_per_segment = sq(theta_per_segment),
raw.k = current_position.k, sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
raw.u = current_position.u, cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
raw.v = current_position.v,
raw.w = current_position.w, #if DISABLED(AUTO_BED_LEVELING_UBL)
raw.e = current_position.e ARC_LIJKUVW_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,
const float per_segment_U = travel_U / segments,
const float per_segment_V = travel_V / segments,
const float per_segment_W = travel_W / segments
);
#endif
#if ENABLED(SCARA_FEEDRATE_SCALING) CODE_ITEM_E(const float extruder_per_segment = travel_E / segments);
const float inv_duration = scaled_fr_mm_s / segment_mm;
#endif
millis_t next_idle_ms = millis() + 200UL; // Initialize all linear axes and E
ARC_LIJKUVWE_CODE(
raw[axis_l] = current_position[axis_l],
raw.i = current_position.i,
raw.j = current_position.j,
raw.k = current_position.k,
raw.u = current_position.u,
raw.v = current_position.v,
raw.w = current_position.w,
raw.e = current_position.e
);
#if N_ARC_CORRECTION > 1 millis_t next_idle_ms = millis() + 200UL;
int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times #if N_ARC_CORRECTION > 1
int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif
thermalManager.manage_heater(); for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
const millis_t ms = millis();
if (ELAPSED(ms, next_idle_ms)) {
next_idle_ms = ms + 200UL;
idle();
}
#if N_ARC_CORRECTION > 1 thermalManager.manage_heater();
if (--arc_recalc_count) { const millis_t ms = millis();
// Apply vector rotation matrix to previous rvec.a / 1 if (ELAPSED(ms, next_idle_ms)) {
const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T; next_idle_ms = ms + 200UL;
rvec.a = rvec.a * cos_T - rvec.b * sin_T; idle();
rvec.b = r_new_Y;
} }
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 #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;
}
// Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments. // Update raw location
// Compute exact location by applying transformation matrix from initial radius vector(=-offset). raw[axis_p] = center_P + rvec.a;
// To reduce stuttering, the sin and cos could be computed at different times. raw[axis_q] = center_Q + rvec.b;
// For now, compute both at the same time. ARC_LIJKUVWE_CODE(
const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment); #if ENABLED(AUTO_BED_LEVELING_UBL)
rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti; raw[axis_l] = start_L,
rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti; raw.i = start_I, raw.j = start_J, raw.k = start_K,
} raw.u = start_U, raw.v = start_V, raw.w = start_V
#else
// Update raw location raw[axis_l] += per_segment_L,
raw[axis_p] = center_P + rvec.a; raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K,
raw[axis_q] = center_Q + rvec.b; raw.u += per_segment_U, raw.v += per_segment_V, raw.w += per_segment_W
ARC_LIJKUVWE_CODE( #endif
#if ENABLED(AUTO_BED_LEVELING_UBL) , raw.e += extruder_per_segment
raw[axis_l] = start_L, );
raw.i = start_I, raw.j = start_J, raw.k = start_K,
raw.u = start_U, raw.v = start_V, raw.w = start_V
#else
raw[axis_l] += per_segment_L,
raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K,
raw.u += per_segment_U, raw.v += per_segment_V, raw.w += per_segment_W
#endif
, raw.e += extruder_per_segment
);
apply_motion_limits(raw); apply_motion_limits(raw);
#if HAS_LEVELING && !PLANNER_LEVELING #if HAS_LEVELING && !PLANNER_LEVELING
planner.apply_leveling(raw); planner.apply_leveling(raw);
#endif #endif
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
break; break;
}
} }
// Ensure last segment arrives at target location. // Ensure last segment arrives at target location.

Write Preview
Loading…
Cancel
Save