Improve plan_arc circle detection (#20440)

Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
yysh12 2020-12-23 00:12:20 -06:00 committed by Scott Lahteine
parent 60aeed99c6
commit ad15890a81

View File

@ -41,13 +41,12 @@
#endif #endif
/** /**
* Plan an arc in 2 dimensions * Plan an arc in 2 dimensions, with optional linear motion in a 3rd dimension
* *
* The arc is approximated by generating many small linear segments. * The arc is traced by generating many small linear segments, as configured by
* The length of each segment is configured in MM_PER_ARC_SEGMENT (Default 1mm) * MM_PER_ARC_SEGMENT (Default 1mm). In the future we hope more slicers will include
* Arcs should only be made relatively large (over 5mm), as larger arcs with * an option to generate G2/G3 arcs for curved surfaces, as this will allow faster
* larger segments will tend to be more efficient. Your slicer should have * boards to produce much smoother curved surfaces.
* options for G2/G3 arc generation. In future these options may be GCode tunable.
*/ */
void plan_arc( void plan_arc(
const xyze_pos_t &cart, // Destination position const xyze_pos_t &cart, // Destination position
@ -77,26 +76,33 @@ 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];
// 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);
#ifdef MIN_ARC_SEGMENTS #ifdef MIN_ARC_SEGMENTS
uint16_t min_segments = MIN_ARC_SEGMENTS; uint16_t min_segments = MIN_ARC_SEGMENTS;
#else #else
constexpr uint16_t min_segments = 1; constexpr uint16_t min_segments = 1;
#endif #endif
// Do a full circle if angular rotation is near 0 and the target is current position // Angle of rotation between position and target from the circle center.
if (!angular_travel || (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis]))) { float angular_travel;
// Do a full circle if starting and ending positions are "identical"
if (NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
// Preserve direction for circles // Preserve direction for circles
angular_travel = clockwise ? -RADIANS(360) : RADIANS(360); angular_travel = clockwise ? -RADIANS(360) : RADIANS(360);
} }
else { else {
// Calculate the angle
angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
// Angular travel too small to detect? Just return.
if (!angular_travel) return;
// Make sure angular travel over 180 degrees goes the other way around. // Make sure angular travel over 180 degrees goes the other way around.
switch (((angular_travel < 0) << 1) | clockwise) { switch (((angular_travel < 0) << 1) | clockwise) {
case 1: angular_travel -= RADIANS(360); break; // Positive but CW? Reverse direction. case 1: angular_travel -= RADIANS(360); break; // Positive but CW? Reverse direction.
case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction. case 2: angular_travel += RADIANS(360); break; // Negative but CCW? Reverse direction.
} }
#ifdef MIN_ARC_SEGMENTS #ifdef MIN_ARC_SEGMENTS
min_segments = CEIL(min_segments * ABS(angular_travel) / RADIANS(360)); min_segments = CEIL(min_segments * ABS(angular_travel) / RADIANS(360));
NOLESS(min_segments, 1U); NOLESS(min_segments, 1U);