From 3557e150cc49e21efb392e9c4facb5207f4dbee2 Mon Sep 17 00:00:00 2001 From: Yash Patel Date: Thu, 26 Nov 2020 12:26:20 -0600 Subject: [PATCH 1/3] G2 and G3 Math Error Correction (#20258) Utilize either CW or CCW angle depending on user specification (via G2/G3) when calculating minimum number of segments (min_segments) --- Marlin/src/gcode/motion/G2_G3.cpp | 26 +++++++++++++++++++++++--- 1 file changed, 23 insertions(+), 3 deletions(-) diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index c713877a0e6a..bd785edfa76e 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -79,14 +79,34 @@ void plan_arc( // 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); + + //ATAN2 gives shortest angle to target vector (i.e. (-pi, +pi] ) // bracket after +pi means inclusive range + // < 0 means CW angle and > 0 means CCW angle -> i.e. -pi (example case since atan2 never returns -pi) equals 180 degrees CW and +pi equals 180 degrees CCW even though both reach same target vector + + // Thus, the only time this should be modified (i.e. through the switch statement below) is if user through specification of either G2 or G3 needs to use the larger angle i.e. > 180 degrees CW or > 180 degress CCW + + switch (((angular_travel < 0)<<1) + clockwise) { + // possible values: + // 0 : means atan2 angle > 0 and user has not specified clockwise motion -> no angular correction needed since atan2 already returns the correct CCW angle + // 1 : means atan2 angle > 0 but user has specified clockwise motion -> thus atan2 returns a CCW angle but we need CW angle. We must subtract 2pi radians to get CW angle. + // 2 : means atan2 angle < 0 but user has specified counter-clockwise motion -> thus atan2 returns a CW angle but we need CCW angle. We must add 2pi radians to get CCW angle. + // 3 : means atan2 angle < 0 and user has not specified counter-clockwise motion -> no angular correction needed since atan2 already returns the correect CW angle + case 1: + angular_travel -= RADIANS(360); + break; + case 2: + angular_travel += RADIANS(360); + break; + } + + + #ifdef MIN_ARC_SEGMENTS - uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360))); + uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (ABS(angular_travel) / RADIANS(360))); // since CW (if specified via G2) angular_travel is negative we must ensure absolute value ABS function is always used here NOLESS(min_segments, 1U); #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 (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) { From 96a03968399e654d3bbf2a287e80f450a7e4d2e7 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 26 Nov 2020 23:16:39 -0600 Subject: [PATCH 2/3] Essentials, labeled. --- Marlin/src/gcode/motion/G2_G3.cpp | 30 ++++++++---------------------- 1 file changed, 8 insertions(+), 22 deletions(-) diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index bd785edfa76e..7c56d4c84cd8 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -77,32 +77,18 @@ void plan_arc( rt_Y = cart[q_axis] - center_Q, start_L = current_position[l_axis]; - // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required. + // Angle of rotation between position and target from the circle center. float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y); - //ATAN2 gives shortest angle to target vector (i.e. (-pi, +pi] ) // bracket after +pi means inclusive range - // < 0 means CW angle and > 0 means CCW angle -> i.e. -pi (example case since atan2 never returns -pi) equals 180 degrees CW and +pi equals 180 degrees CCW even though both reach same target vector - - // Thus, the only time this should be modified (i.e. through the switch statement below) is if user through specification of either G2 or G3 needs to use the larger angle i.e. > 180 degrees CW or > 180 degress CCW - - switch (((angular_travel < 0)<<1) + clockwise) { - // possible values: - // 0 : means atan2 angle > 0 and user has not specified clockwise motion -> no angular correction needed since atan2 already returns the correct CCW angle - // 1 : means atan2 angle > 0 but user has specified clockwise motion -> thus atan2 returns a CCW angle but we need CW angle. We must subtract 2pi radians to get CW angle. - // 2 : means atan2 angle < 0 but user has specified counter-clockwise motion -> thus atan2 returns a CW angle but we need CCW angle. We must add 2pi radians to get CCW angle. - // 3 : means atan2 angle < 0 and user has not specified counter-clockwise motion -> no angular correction needed since atan2 already returns the correect CW angle - case 1: - angular_travel -= RADIANS(360); - break; - case 2: - angular_travel += RADIANS(360); - break; - } - - + // Make sure angular travel over 180 degrees goes the other way around. + const uint8_t congruity = ((angular_travel < 0) << 1) + clockwise; + if (congruity == 1) + angular_travel -= RADIANS(360); // Positive but CW? Reverse direction. + else if (congruity == 2) + angular_travel += RADIANS(360); // Negative but CCW? Reverse direction. #ifdef MIN_ARC_SEGMENTS - uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (ABS(angular_travel) / RADIANS(360))); // since CW (if specified via G2) angular_travel is negative we must ensure absolute value ABS function is always used here + uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * ABS(angular_travel) / RADIANS(360)); NOLESS(min_segments, 1U); #else constexpr uint16_t min_segments = 1; From 258ae916a09c67557b5fde7ee5e89ec5ba3ec1c8 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Thu, 26 Nov 2020 23:18:22 -0600 Subject: [PATCH 3/3] Keep switch --- Marlin/src/gcode/motion/G2_G3.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index 7c56d4c84cd8..b920e2307336 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -81,11 +81,10 @@ void plan_arc( float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y); // Make sure angular travel over 180 degrees goes the other way around. - const uint8_t congruity = ((angular_travel < 0) << 1) + clockwise; - if (congruity == 1) - angular_travel -= RADIANS(360); // Positive but CW? Reverse direction. - else if (congruity == 2) - angular_travel += RADIANS(360); // Negative but CCW? Reverse direction. + 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. + } #ifdef MIN_ARC_SEGMENTS uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * ABS(angular_travel) / RADIANS(360));