From a596969049a8522c630ec064e727e5277b6f1cff Mon Sep 17 00:00:00 2001 From: uwedamm <46942357+uwedamm@users.noreply.github.com> Date: Wed, 21 Oct 2020 03:26:07 +0200 Subject: [PATCH] Fix G2/G3 P E and Z motion (#19797) Co-authored-by: Scott Lahteine --- Marlin/src/gcode/bedlevel/G26.cpp | 4 ++-- Marlin/src/gcode/motion/G2_G3.cpp | 39 ++++++++++++++++++++++--------- 2 files changed, 30 insertions(+), 13 deletions(-) diff --git a/Marlin/src/gcode/bedlevel/G26.cpp b/Marlin/src/gcode/bedlevel/G26.cpp index 6123fbf6c8..1c0d8cba8e 100644 --- a/Marlin/src/gcode/bedlevel/G26.cpp +++ b/Marlin/src/gcode/bedlevel/G26.cpp @@ -136,7 +136,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&, const ab_float_t&, const bool, const uint8_t); #endif constexpr float g26_e_axis_feedrate = 0.025; @@ -783,7 +783,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 61d9f1d3a6..c713877a0e 100644 --- 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; @@ -74,9 +75,7 @@ void plan_arc( center_Q = current_position[q_axis] - rvec.b, rt_X = cart[p_axis] - center_P, rt_Y = cart[q_axis] - center_Q, - start_L = current_position[l_axis], - linear_travel = cart[l_axis] - start_L, - extruder_travel = cart.e - current_position.e; + start_L = current_position[l_axis]; // 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); @@ -90,13 +89,32 @@ void plan_arc( 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]) { + if (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) { angular_travel = RADIANS(360); #ifdef MIN_ARC_SEGMENTS min_segments = MIN_ARC_SEGMENTS; #endif } + float linear_travel = cart[l_axis] - start_L, + extruder_travel = cart.e - current_position.e; + + // If circling around... + if (ENABLED(ARC_P_CIRCLES) && circles) { + const float total_angular = 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 + l_per_circle = linear_travel * part_per_circle, // L movement per circle + e_per_circle = extruder_travel * part_per_circle; // E movement per circle + xyze_pos_t temp_position = current_position; // for plan_arc to compare to current_position + for (uint16_t n = circles; n--;) { + temp_position.e += e_per_circle; // Destination E axis + temp_position[l_axis] += l_per_circle; // Destination L axis + plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle + } + linear_travel = cart[l_axis] - current_position[l_axis]; + extruder_travel = cart.e - current_position.e; + } + const float flat_mm = radius * angular_travel, mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm); if (mm_of_travel < 0.001f) return; @@ -150,7 +168,7 @@ void plan_arc( 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, + 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 @@ -320,16 +338,15 @@ void GcodeSuite::G2_G3(const bool clockwise) { #if ENABLED(ARC_P_CIRCLES) // P indicates number of circles to do - int8_t circles_to_do = parser.byteval('P'); + const 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