Improve plan_arc circle detection (#20440)
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
parent
60aeed99c6
commit
ad15890a81
@ -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);
|
||||||
|
Loading…
Reference in New Issue
Block a user