|
@ -80,24 +80,26 @@ void plan_arc( |
|
|
// Angle of rotation between position and target from the circle center.
|
|
|
// 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); |
|
|
|
|
|
|
|
|
// 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) * ABS(angular_travel) / RADIANS(360)); |
|
|
uint16_t min_segments = MIN_ARC_SEGMENTS; |
|
|
NOLESS(min_segments, 1U); |
|
|
|
|
|
#else |
|
|
#else |
|
|
constexpr uint16_t min_segments = 1; |
|
|
constexpr uint16_t min_segments = 1; |
|
|
#endif |
|
|
#endif |
|
|
|
|
|
|
|
|
// Make a circle if the angular rotation is 0 and the target is current position
|
|
|
// Do a full circle if angular rotation is near 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 ((!angular_travel || NEAR_ZERO(angular_travel)) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) { |
|
|
angular_travel = RADIANS(360); |
|
|
// Preserve direction for circles
|
|
|
|
|
|
angular_travel = clockwise ? -RADIANS(360) : RADIANS(360); |
|
|
|
|
|
} |
|
|
|
|
|
else { |
|
|
|
|
|
// 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 |
|
|
min_segments = MIN_ARC_SEGMENTS; |
|
|
min_segments = CEIL(min_segments * ABS(angular_travel) / RADIANS(360)); |
|
|
|
|
|
NOLESS(min_segments, 1U); |
|
|
#endif |
|
|
#endif |
|
|
} |
|
|
} |
|
|
|
|
|
|
|
|