diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index 0c7bdc7a3a..85e8c5ff64 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -179,8 +179,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 @@ -188,19 +188,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, @@ -228,98 +227,100 @@ 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_LIJK_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 + // 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_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); - - // 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(); - } + millis_t next_idle_ms = millis() + 200UL; #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; + 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(); } - 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; + } + + // 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, + 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); + + #if HAS_LEVELING && !PLANNER_LEVELING + planner.apply_leveling(raw); #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; + if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) + break; } - - // 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.