wp/Marlin-2-0-x-Anycubic-i3-MEGA-S
1
0
Fork 0
Code Issues 28 Pull Requests 2 Actions Packages Projects Releases 30 Wiki Activity

backport G2/G3 changes from upstream to fix ARC issues

Browse Source
This commit is contained in:
Ashley Sommer
2021-08-20 15:41:54 +10:00
parent b3f4dcb2d1
commit d464e036e0
2 changed files with 78 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Marlin/src/gcode/bedlevel/G26.cpp
View File

@@ -67,7 +67,7 @@
#define G26_ERR true
#if ENABLED(ARC_SUPPORT)
void plan_arc(const xyze_pos_t &cart, const ab_float_t &offset, const uint8_t clockwise);
void plan_arc(const xyze_pos_t &cart, const ab_float_t &offset, const bool clockwise, const uint8_t);
#endif
/**
@@ -886,7 +886,7 @@ void GcodeSuite::G26()
const feedRate_t old_feedrate = feedrate_mm_s;
feedrate_mm_s = PLANNER_XY_FEEDRATE() * 0.1f;
plan_arc(endpoint, arc_offset, false); // Draw a counter-clockwise arc
plan_arc(endpoint, arc_offset, false, 0); // Draw a counter-clockwise arc
feedrate_mm_s = old_feedrate;
destination = current_position;

100
Marlin/src/gcode/motion/G2_G3.cpp
View File

@@ -52,7 +52,8 @@
void plan_arc(
const xyze_pos_t &cart, // Destination position
const ab_float_t &offset, // Center of rotation relative to current_position
const uint8_t clockwise // Clockwise?
const bool clockwise, // Clockwise?
const uint8_t circles // Take the scenic route
) {
#if ENABLED(CNC_WORKSPACE_PLANES)
AxisEnum p_axis, q_axis, l_axis;
@@ -70,52 +71,91 @@ void plan_arc(
ab_float_t rvec = -offset;
const float radius = HYPOT(rvec.a, rvec.b),
#if ENABLED(AUTO_BED_LEVELING_UBL)
start_L = current_position[l_axis],
#endif
center_P = current_position[p_axis] - rvec.a,
center_Q = current_position[q_axis] - rvec.b,
rt_X = cart[p_axis] - center_P,
rt_Y = cart[q_axis] - center_Q,
linear_travel = cart[l_axis] - current_position[l_axis],
start_L = current_position[l_axis],
linear_travel = cart[l_axis] - start_L,
extruder_travel = cart.e - current_position.e;
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
if (angular_travel < 0) angular_travel += RADIANS(360);
#ifdef MIN_ARC_SEGMENTS
uint16_t min_segments = CEIL((MIN_ARC_SEGMENTS) * (angular_travel / RADIANS(360)));
NOLESS(min_segments, 1U);
uint16_t min_segments = MIN_ARC_SEGMENTS;
#else
constexpr uint16_t min_segments = 1;
#endif
if (clockwise) angular_travel -= RADIANS(360);
// Make a circle if the angular rotation is 0 and the target is current position
if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) {
angular_travel = RADIANS(360);
// Angle of rotation between position and target from the circle center.
float angular_travel, abs_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
angular_travel = clockwise ? -RADIANS(360) : RADIANS(360);
abs_angular_travel = RADIANS(360);
}
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.
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.
}
abs_angular_travel = ABS(angular_travel);
#ifdef MIN_ARC_SEGMENTS
min_segments = 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);
#endif
}
const float flat_mm = radius * angular_travel,
mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
if (ENABLED(ARC_P_CIRCLES) && circles) {
const float total_angular = abs_angular_travel + circles * RADIANS(360), // Total rotation with all circles and remainder
part_per_circle = RADIANS(360) / total_angular; // Each circle's part of the total
#if HAS_Z_AXIS
const float l_per_circle = linear_travel * part_per_circle; // L movement per circle
#endif
#if HAS_EXTRUDERS
const float e_per_circle = extruder_travel * part_per_circle; // E movement per circle
#endif
xyze_pos_t temp_position = current_position; // for plan_arc to compare to current_position
for (uint16_t n = circles; n--;) {
TERN_(HAS_EXTRUDERS, temp_position.e += e_per_circle); // Destination E axis
TERN_(HAS_Z_AXIS, temp_position[l_axis] += l_per_circle); // Destination L axis
plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle
}
TERN_(HAS_Z_AXIS, linear_travel = cart[l_axis] - current_position[l_axis]);
TERN_(HAS_EXTRUDERS, extruder_travel = cart.e - current_position.e);
}
const float flat_mm = radius * abs_angular_travel,
mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : flat_mm;
if (mm_of_travel < 0.001f) return;
const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
// Start with a nominal segment length
float seg_length = (
#ifdef ARC_SEGMENTS_PER_R
float seg_length = MM_PER_ARC_SEGMENT * radius;
LIMIT(seg_length, MM_PER_ARC_SEGMENT, ARC_SEGMENTS_PER_R);
constrain(MM_PER_ARC_SEGMENT * radius, MM_PER_ARC_SEGMENT, ARC_SEGMENTS_PER_R)
#elif ARC_SEGMENTS_PER_SEC
float seg_length = scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC);
NOLESS(seg_length, MM_PER_ARC_SEGMENT);
_MAX(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MM_PER_ARC_SEGMENT)
#else
constexpr float seg_length = MM_PER_ARC_SEGMENT;
MM_PER_ARC_SEGMENT
#endif
);
// Divide total travel by nominal segment length
uint16_t segments = FLOOR(mm_of_travel / seg_length);
NOLESS(segments, min_segments);
NOLESS(segments, min_segments); // At least some segments
seg_length = mm_of_travel / segments;
/**
* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
@@ -148,8 +188,9 @@ void plan_arc(
const float theta_per_segment = angular_travel / segments,
linear_per_segment = linear_travel / segments,
extruder_per_segment = extruder_travel / segments,
sin_T = theta_per_segment,
cos_T = 1 - 0.5f * sq(theta_per_segment); // Small angle approximation
sq_theta_per_segment = sq(theta_per_segment),
sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
// Initialize the linear axis
raw[l_axis] = current_position[l_axis];
@@ -283,7 +324,7 @@ void GcodeSuite::G2_G3(const bool clockwise) {
relative_mode = true;
#endif
get_destination_from_command();
get_destination_from_command(); // Get X Y Z E F (and set cutter power)
#if ENABLED(SF_ARC_FIX)
relative_mode = relative_mode_backup;
@@ -328,13 +369,12 @@ void GcodeSuite::G2_G3(const bool clockwise) {
int8_t circles_to_do = parser.byteval('P');
if (!WITHIN(circles_to_do, 0, 100))
SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS);
while (circles_to_do--)
plan_arc(current_position, arc_offset, clockwise);
#else
constexpr uint8_t circles_to_do = 0;
#endif
// Send the arc to the planner
plan_arc(destination, arc_offset, clockwise);
plan_arc(destination, arc_offset, clockwise, circles_to_do);
reset_stepper_timeout();
}
else