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
Split 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
const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
// Get the nominal segment length based on settings
const float nominal_segment_mm = (
// Get the ideal segment length for the move based on settings
const float ideal_segment_mm = (
#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)
#else
@ -206,19 +206,18 @@ void plan_arc(
#endif
);
// Number of whole segments based on the nominal segment length
const float nominal_segments = _MAX(FLOOR(flat_mm / nominal_segment_mm), min_segments);
// Number of whole segments based on the ideal segment length
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
const float segment_mm = constrain(flat_mm / nominal_segments, MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM);
// The number of whole segments in the arc, with best attempt to honor MIN_ARC_SEGMENT_MM and 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
uint16_t segments = FLOOR(flat_mm / segment_mm);
// 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;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
#endif
/**
* 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.
* 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);
// For shortened segments, run all but the remainder in the loop
if (tooshort) segments++;
xyze_pos_t raw;
// 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
);
// do not calculate rotation parameters for trivial single-segment arcs
if (segments > 1) {
// Vector rotation matrix values
const float theta_per_segment = 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 = 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)
const float inv_duration = scaled_fr_mm_s / segment_mm;
#endif
CODE_ITEM_E(const float extruder_per_segment = travel_E / segments);
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
int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif
millis_t next_idle_ms = millis() + 200UL;
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();
const millis_t ms = millis();
if (ELAPSED(ms, next_idle_ms)) {
next_idle_ms = ms + 200UL;
idle();
}
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
#if N_ARC_CORRECTION > 1
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;
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
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;
}
// 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_LIJKUVWE_CODE(
#if ENABLED(AUTO_BED_LEVELING_UBL)
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
);
// Update raw location
raw[axis_p] = center_P + rvec.a;
raw[axis_q] = center_Q + rvec.b;
ARC_LIJKUVWE_CODE(
#if ENABLED(AUTO_BED_LEVELING_UBL)
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
planner.apply_leveling(raw);
#endif
#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;
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.

Write Preview
Loading…
Cancel
Save