File tree Expand file tree Collapse file tree 1 file changed +9
-4
lines changed
Expand file tree Collapse file tree 1 file changed +9
-4
lines changed Original file line number Diff line number Diff line change @@ -77,16 +77,21 @@ void plan_arc(
7777 rt_Y = cart[q_axis] - center_Q,
7878 start_L = current_position[l_axis];
7979
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.
8181 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+
8389 #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 ));
8591 NOLESS (min_segments, 1U );
8692 #else
8793 constexpr uint16_t min_segments = 1 ;
8894 #endif
89- if (clockwise) angular_travel -= RADIANS (360 );
9095
9196 // Make a circle if the angular rotation is 0 and the target is current position
9297 if (NEAR_ZERO (angular_travel) && NEAR (current_position[p_axis], cart[p_axis]) && NEAR (current_position[q_axis], cart[q_axis])) {
You can’t perform that action at this time.
0 commit comments