diff --git a/Marlin/src/gcode/bedlevel/G26.cpp b/Marlin/src/gcode/bedlevel/G26.cpp index 457f5b17..862d9ffa 100755 --- a/Marlin/src/gcode/bedlevel/G26.cpp +++ b/Marlin/src/gcode/bedlevel/G26.cpp @@ -67,7 +67,7 @@ #define G26_ERR true #if ENABLED(ARC_SUPPORT) -void plan_arc(const xyze_pos_t &cart, const ab_float_t &offset, const uint8_t clockwise); +void plan_arc(const xyze_pos_t &cart, const ab_float_t &offset, const bool clockwise, const uint8_t); #endif /** @@ -886,7 +886,7 @@ void GcodeSuite::G26() const feedRate_t old_feedrate = feedrate_mm_s; feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f; - plan_arc(endpoint, arc_offset, false); // Draw a counter-clockwise arc + plan_arc(endpoint, arc_offset, false, 0); // Draw a counter-clockwise arc feedrate_mm_s = old_feedrate; destination = current_position; diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index 19c0eacd..e51aff92 100755 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -52,7 +52,8 @@ void plan_arc( const xyze_pos_t &cart, // Destination position const ab_float_t &offset, // Center of rotation relative to current_position - const uint8_t clockwise // Clockwise? + const bool clockwise, // Clockwise? + const uint8_t circles // Take the scenic route ) { #if ENABLED(CNC_WORKSPACE_PLANES) AxisEnum p_axis, q_axis, l_axis; @@ -70,52 +71,91 @@ void plan_arc( ab_float_t rvec = -offset; const float radius = HYPOT(rvec.a, rvec.b), - #if ENABLED(AUTO_BED_LEVELING_UBL) - start_L = current_position[l_axis], - #endif center_P = current_position[p_axis] - rvec.a, center_Q = current_position[q_axis] - rvec.b, rt_X = cart[p_axis] - center_P, rt_Y = cart[q_axis] - center_Q, - linear_travel = cart[l_axis] - current_position[l_axis], + start_L = current_position[l_axis], + linear_travel = cart[l_axis] - start_L, extruder_travel = cart.e - current_position.e; - // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required. - float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y); - if (angular_travel < 0) angular_travel += RADIANS(360); #ifdef MIN_ARC_SEGMENTS - uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360))); - NOLESS(min_segments, 1U); + uint16_t min_segments = MIN_ARC_SEGMENTS; #else constexpr uint16_t min_segments = 1; #endif - if (clockwise) angular_travel -= RADIANS(360); - // Make a circle if the angular rotation is 0 and the target is current position - if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) { - angular_travel = RADIANS(360); + // Angle of rotation between position and target from the circle center. + float angular_travel, abs_angular_travel; + + // Do a full circle if starting and ending positions are "identical" + if (NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) { + // Preserve direction for circles + angular_travel = clockwise ? -RADIANS(360) : RADIANS(360); + abs_angular_travel = RADIANS(360); + } + else { + // Calculate the angle + angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y); + // Angular travel too small to detect? Just return. + if (!angular_travel) return; + // Make sure angular travel over 180 degrees goes the other way around. + switch (((angular_travel < 0) << 1) | clockwise) { + case 1: angular_travel -= RADIANS(360); break; // Positive but CW? Reverse direction. + case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction. + } + + abs_angular_travel = ABS(angular_travel); + #ifdef MIN_ARC_SEGMENTS - min_segments = MIN_ARC_SEGMENTS; + min_segments = CEIL(min_segments * ABS(angular_travel) / RADIANS(360)); + min_segments = CEIL(min_segments * abs_angular_travel / RADIANS(360)); + NOLESS(min_segments, 1U); #endif } - const float flat_mm = radius * angular_travel, - mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm); + if (ENABLED(ARC_P_CIRCLES) && circles) { + const float total_angular = abs_angular_travel + circles * RADIANS(360), // Total rotation with all circles and remainder + part_per_circle = RADIANS(360) / total_angular; // Each circle's part of the total + + #if HAS_Z_AXIS + const float l_per_circle = linear_travel * part_per_circle; // L movement per circle + #endif + #if HAS_EXTRUDERS + const float e_per_circle = extruder_travel * part_per_circle; // E movement per circle + #endif + xyze_pos_t temp_position = current_position; // for plan_arc to compare to current_position + for (uint16_t n = circles; n--;) { + TERN_(HAS_EXTRUDERS, temp_position.e += e_per_circle); // Destination E axis + TERN_(HAS_Z_AXIS, temp_position[l_axis] += l_per_circle); // Destination L axis + plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle + } + TERN_(HAS_Z_AXIS, linear_travel = cart[l_axis] - current_position[l_axis]); + TERN_(HAS_EXTRUDERS, extruder_travel = cart.e - current_position.e); + } + + + + const float flat_mm = radius * abs_angular_travel, + mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : flat_mm; if (mm_of_travel < 0.001f) return; const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s); - #ifdef ARC_SEGMENTS_PER_R - float seg_length = MM_PER_ARC_SEGMENT * radius; - LIMIT(seg_length, MM_PER_ARC_SEGMENT, ARC_SEGMENTS_PER_R); - #elif ARC_SEGMENTS_PER_SEC - float seg_length = scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC); - NOLESS(seg_length, MM_PER_ARC_SEGMENT); - #else - constexpr float seg_length = MM_PER_ARC_SEGMENT; - #endif + // Start with a nominal segment length + float seg_length = ( + #ifdef ARC_SEGMENTS_PER_R + constrain(MM_PER_ARC_SEGMENT * radius, MM_PER_ARC_SEGMENT, ARC_SEGMENTS_PER_R) + #elif ARC_SEGMENTS_PER_SEC + _MAX(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MM_PER_ARC_SEGMENT) + #else + MM_PER_ARC_SEGMENT + #endif + ); + // Divide total travel by nominal segment length uint16_t segments = FLOOR(mm_of_travel / seg_length); - NOLESS(segments, min_segments); + NOLESS(segments, min_segments); // At least some segments + seg_length = mm_of_travel / segments; /** * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, @@ -146,10 +186,11 @@ void plan_arc( // Vector rotation matrix values xyze_pos_t raw; const float theta_per_segment = angular_travel / segments, - linear_per_segment = linear_travel / segments, - extruder_per_segment = extruder_travel / segments, - sin_T = theta_per_segment, - cos_T = 1 - 0.5f * sq(theta_per_segment); // Small angle approximation + linear_per_segment = linear_travel / segments, + extruder_per_segment = extruder_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 // Initialize the linear axis raw[l_axis] = current_position[l_axis]; @@ -283,7 +324,7 @@ void GcodeSuite::G2_G3(const bool clockwise) { relative_mode = true; #endif - get_destination_from_command(); + get_destination_from_command(); // Get X Y Z E F (and set cutter power) #if ENABLED(SF_ARC_FIX) relative_mode = relative_mode_backup; @@ -328,13 +369,12 @@ void GcodeSuite::G2_G3(const bool clockwise) { int8_t circles_to_do = parser.byteval('P'); if (!WITHIN(circles_to_do, 0, 100)) SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS); - - while (circles_to_do--) - plan_arc(current_position, arc_offset, clockwise); + #else + constexpr uint8_t circles_to_do = 0; #endif // Send the arc to the planner - plan_arc(destination, arc_offset, clockwise); + plan_arc(destination, arc_offset, clockwise, circles_to_do); reset_stepper_timeout(); } else