Browse Source

Fix G2/G3 arcs > 180° (#20292)

yysh12 4 years ago
parent
commit
bab660ca7d
No account linked to committer's email address
1 changed files with 9 additions and 4 deletions
  1. 9
    4
      Marlin/src/gcode/motion/G2_G3.cpp

+ 9
- 4
Marlin/src/gcode/motion/G2_G3.cpp View File

77
               rt_Y = cart[q_axis] - center_Q,
77
               rt_Y = cart[q_axis] - center_Q,
78
               start_L = current_position[l_axis];
78
               start_L = current_position[l_axis];
79
 
79
 
80
-  // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
80
+  // Angle of rotation between position and target from the circle center.
81
   float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
81
   float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
82
-  if (angular_travel < 0) angular_travel += RADIANS(360);
82
+
83
+  // Make sure angular travel over 180 degrees goes the other way around.
84
+  switch (((angular_travel < 0) << 1) + clockwise) {
85
+    case 1: angular_travel -= RADIANS(360); break; // Positive but CW? Reverse direction.
86
+    case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction.
87
+  }
88
+
83
   #ifdef MIN_ARC_SEGMENTS
89
   #ifdef MIN_ARC_SEGMENTS
84
-    uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360)));
90
+    uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * ABS(angular_travel) / RADIANS(360));
85
     NOLESS(min_segments, 1U);
91
     NOLESS(min_segments, 1U);
86
   #else
92
   #else
87
     constexpr uint16_t min_segments = 1;
93
     constexpr uint16_t min_segments = 1;
88
   #endif
94
   #endif
89
-  if (clockwise) angular_travel -= RADIANS(360);
90
 
95
 
91
   // Make a circle if the angular rotation is 0 and the target is current position
96
   // Make a circle if the angular rotation is 0 and the target is current position
92
   if (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
97
   if (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {

Loading…
Cancel
Save