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

This commit is contained in:
yysh12 2020-11-26 23:29:07 -06:00 committed by GitHub
parent 109f68f7df
commit bab660ca7d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23

View File

@ -77,16 +77,21 @@ void plan_arc(
rt_Y = cart[q_axis] - center_Q, rt_Y = cart[q_axis] - center_Q,
start_L = current_position[l_axis]; 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); 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);
// 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.
}
#ifdef MIN_ARC_SEGMENTS #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));
NOLESS(min_segments, 1U); NOLESS(min_segments, 1U);
#else #else
constexpr uint16_t min_segments = 1; constexpr uint16_t min_segments = 1;
#endif #endif
if (clockwise) angular_travel -= RADIANS(360);
// Make a circle if the angular rotation is 0 and the target is current position // 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])) { if (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {