Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

G2/G3: Fix Z/helix planning on UBL #26170

Merged
merged 4 commits into from
Aug 21, 2023
Merged

G2/G3: Fix Z/helix planning on UBL #26170

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
eaf160e
G2/G3: Fix Z/helix planning on UBL
wavexx Aug 11, 2023
76728a9
G2/G3: Reduce error in safe_exit_speed_sqr calculation at high resolu…
wavexx Aug 11, 2023
8d756f8
update comments
thinkyhead Aug 20, 2023
4d1fffb
Merge branch 'bugfix-2.1.x' into pr/26170
thinkyhead Aug 20, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
130 changes: 64 additions & 66 deletions Marlin/src/gcode/motion/G2_G3.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -82,14 +82,17 @@ void plan_arc(
rt_X = cart[axis_p] - center_P,
rt_Y = cart[axis_q] - center_Q;

ARC_LIJKUVW_CODE(
const float start_L = current_position[axis_l],
const float start_I = current_position.i,
const float start_J = current_position.j,
const float start_K = current_position.k,
const float start_U = current_position.u,
const float start_V = current_position.v,
const float start_W = current_position.w
// Starting position of the move for all non-arc axes
// i.e., only one of X, Y, or Z, plus the rest.
ARC_LIJKUVWE_CODE(
float start_L = current_position[axis_l],
float start_I = current_position.i,
float start_J = current_position.j,
float start_K = current_position.k,
float start_U = current_position.u,
float start_V = current_position.v,
float start_W = current_position.w,
float start_E = current_position.e
);

// Angle of rotation between position and target from the circle center.
Expand Down Expand Up @@ -125,6 +128,7 @@ void plan_arc(
min_segments = CEIL((MIN_CIRCLE_SEGMENTS) * portion_of_circle); // Minimum segments for the arc
}

// Total travel on all the non-arc axes
ARC_LIJKUVWE_CODE(
float travel_L = cart[axis_l] - start_L,
float travel_I = cart.i - start_I,
Expand All @@ -133,7 +137,7 @@ void plan_arc(
float travel_U = cart.u - start_U,
float travel_V = cart.v - start_V,
float travel_W = cart.w - start_W,
float travel_E = cart.e - current_position.e
float travel_E = cart.e - start_E
);

// If "P" specified circles, call plan_arc recursively then continue with the rest of the arc
Expand Down Expand Up @@ -166,15 +170,29 @@ void plan_arc(
);
plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle
}

// Get starting coordinates for the remainder from the current position
ARC_LIJKUVWE_CODE(
start_L = current_position[axis_l],
start_I = current_position.i,
start_J = current_position.j,
start_K = current_position.k,
start_U = current_position.u,
start_V = current_position.v,
start_W = current_position.w,
start_E = current_position.e
);

// Update travel distance for the remainder
ARC_LIJKUVWE_CODE(
travel_L = cart[axis_l] - current_position[axis_l], // Linear X, Y, or Z
travel_I = cart.i - current_position.i, // The rest are also non-arc
travel_J = cart.j - current_position.j,
travel_K = cart.k - current_position.k,
travel_U = cart.u - current_position.u,
travel_V = cart.v - current_position.v,
travel_W = cart.w - current_position.w,
travel_E = cart.e - current_position.e
travel_L = cart[axis_l] - start_L, // Linear X, Y, or Z
travel_I = cart.i - start_I, // The rest are also non-arc
travel_J = cart.j - start_J,
travel_K = cart.k - start_K,
travel_U = cart.u - start_U,
travel_V = cart.v - start_V,
travel_W = cart.w - start_W,
travel_E = cart.e - start_E
);
}

Expand Down Expand Up @@ -256,38 +274,35 @@ void plan_arc(

xyze_pos_t raw;

// do not calculate rotation parameters for trivial single-segment arcs
// Don't calculate rotation parameters for trivial single-segment arcs
if (segments > 1) {
// Vector rotation matrix values
const float theta_per_segment = angular_travel / segments,
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

#if DISABLED(AUTO_BED_LEVELING_UBL)
ARC_LIJKUVW_CODE(
const float per_segment_L = travel_L / segments,
const float per_segment_I = travel_I / segments,
const float per_segment_J = travel_J / segments,
const float per_segment_K = travel_K / segments,
const float per_segment_U = travel_U / segments,
const float per_segment_V = travel_V / segments,
const float per_segment_W = travel_W / segments
);
#endif

CODE_ITEM_E(const float extruder_per_segment = travel_E / segments);
ARC_LIJKUVWE_CODE(
const float per_segment_L = travel_L / segments,
const float per_segment_I = travel_I / segments,
const float per_segment_J = travel_J / segments,
const float per_segment_K = travel_K / segments,
const float per_segment_U = travel_U / segments,
const float per_segment_V = travel_V / segments,
const float per_segment_W = travel_W / segments,
const float per_segment_E = travel_E / segments
);

// Initialize all linear axes and E
ARC_LIJKUVWE_CODE(
raw[axis_l] = current_position[axis_l],
raw.i = current_position.i,
raw.j = current_position.j,
raw.k = current_position.k,
raw.u = current_position.u,
raw.v = current_position.v,
raw.w = current_position.w,
raw.e = current_position.e
raw[axis_l] = start_L,
raw.i = start_I,
raw.j = start_J,
raw.k = start_K,
raw.u = start_U,
raw.v = start_V,
raw.w = start_W,
raw.e = start_E
);

millis_t next_idle_ms = millis() + 200UL;
Expand All @@ -305,7 +320,6 @@ void plan_arc(
const float limiting_accel = _MIN(planner.settings.max_acceleration_mm_per_s2[axis_p], planner.settings.max_acceleration_mm_per_s2[axis_q]),
limiting_speed = _MIN(planner.settings.max_feedrate_mm_s[axis_p], planner.settings.max_feedrate_mm_s[axis_q]),
limiting_speed_sqr = _MIN(sq(limiting_speed), limiting_accel * radius, sq(scaled_fr_mm_s));
float arc_mm_remaining = flat_mm;

for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times

Expand Down Expand Up @@ -343,16 +357,14 @@ void plan_arc(
raw[axis_p] = center_P + rvec.a;
raw[axis_q] = center_Q + rvec.b;
ARC_LIJKUVWE_CODE(
#if ENABLED(AUTO_BED_LEVELING_UBL)
raw[axis_l] = start_L,
raw.i = start_I, raw.j = start_J, raw.k = start_K,
raw.u = start_U, raw.v = start_V, raw.w = start_V
#else
raw[axis_l] += per_segment_L,
raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K,
raw.u += per_segment_U, raw.v += per_segment_V, raw.w += per_segment_W
#endif
, raw.e += extruder_per_segment
raw[axis_l] = start_L + per_segment_L * i,
raw.i = start_I + per_segment_I * i,
raw.j = start_J + per_segment_J * i,
raw.k = start_K + per_segment_K * i,
raw.u = start_U + per_segment_U * i,
raw.v = start_V + per_segment_V * i,
raw.w = start_W + per_segment_W * i,
raw.e = start_E + per_segment_E * i
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This introduces potentially a lot of extra multiplication –at least two for the most common case– which would be nice to try and avoid. Is multiplication actually more accurate than successive additions?

);

apply_motion_limits(raw);
Expand All @@ -362,7 +374,7 @@ void plan_arc(
#endif

// calculate safe speed for stopping by the end of the arc
arc_mm_remaining -= segment_mm;
const float arc_mm_remaining = flat_mm - segment_mm * i;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As above, is multiplication actually more accurate than successive subtractions?

hints.safe_exit_speed_sqr = _MIN(limiting_speed_sqr, 2 * limiting_accel * arc_mm_remaining);

if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
Expand All @@ -374,13 +386,6 @@ void plan_arc(

// Ensure last segment arrives at target location.
raw = cart;
#if ENABLED(AUTO_BED_LEVELING_UBL)
ARC_LIJKUVW_CODE(
raw[axis_l] = start_L,
raw.i = start_I, raw.j = start_J, raw.k = start_K,
raw.u = start_U, raw.v = start_V, raw.w = start_W
);
#endif

apply_motion_limits(raw);

Expand All @@ -392,14 +397,7 @@ void plan_arc(
hints.safe_exit_speed_sqr = 0.0f;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);

#if ENABLED(AUTO_BED_LEVELING_UBL)
ARC_LIJKUVW_CODE(
raw[axis_l] = start_L,
raw.i = start_I, raw.j = start_J, raw.k = start_K,
raw.u = start_U, raw.v = start_V, raw.w = start_W
);
#endif
current_position = raw;
current_position = cart;

} // plan_arc

Expand Down