|
|
|
|
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])) {
|