Skip to content

Commit

Permalink
⚡️ Optimize G2-G3 Arcs (MarlinFirmware#24366)
Browse files Browse the repository at this point in the history
  • Loading branch information
@tombrazier @LCh-77
tombrazier authored and LCh-77 committed Jul 10, 2022
1 parent 2268452 commit 6bd244a
Show file tree
Hide file tree
Showing 8 changed files with 327 additions and 272 deletions.
17 changes: 7 additions & 10 deletions Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -374,11 +374,12 @@
#endif

NOLESS(segments, 1U); // Must have at least one segment
const float inv_segments = 1.0f / segments, // Reciprocal to save calculation
segment_xyz_mm = SQRT(cart_xy_mm_2 + sq(total.z)) * inv_segments; // Length of each segment
const float inv_segments = 1.0f / segments; // Reciprocal to save calculation

// Add hints to help optimize the move
PlannerHints hints(SQRT(cart_xy_mm_2 + sq(total.z)) * inv_segments); // Length of each segment
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / segment_xyz_mm;
hints.inv_duration = scaled_fr_mm_s / hints.millimeters;
#endif

xyze_float_t diff = total * inv_segments;
Expand All @@ -392,13 +393,9 @@
if (!planner.leveling_active || !planner.leveling_active_at_z(destination.z)) {
while (--segments) {
raw += diff;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
);
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
}
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, segment_xyz_mm
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
);
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, hints);
return false; // Did not set current from destination
}

Expand Down Expand Up @@ -467,7 +464,7 @@
TERN_(ENABLE_LEVELING_FADE_HEIGHT, * fade_scaling_factor); // apply fade factor to interpolated height

const float oldz = raw.z; raw.z += z_cxcy;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration) );
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
raw.z = oldz;

if (segments == 0) // done with last segment
Expand Down
3 changes: 2 additions & 1 deletion Marlin/src/feature/joystick.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -172,8 +172,9 @@ Joystick joystick;
current_position += move_dist;
apply_motion_limits(current_position);
const float length = sqrt(hypot2);
PlannerHints hints(length);
injecting_now = true;
planner.buffer_line(current_position, length / seg_time, active_extruder, length);
planner.buffer_line(current_position, length / seg_time, active_extruder, hints);
injecting_now = false;
}
}
Expand Down
26 changes: 23 additions & 3 deletions Marlin/src/gcode/motion/G2_G3.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,9 +214,12 @@ void plan_arc(
const uint16_t segments = nominal_segment_mm > (MAX_ARC_SEGMENT_MM) ? CEIL(flat_mm / (MAX_ARC_SEGMENT_MM)) :
nominal_segment_mm < (MIN_ARC_SEGMENT_MM) ? _MAX(1, FLOOR(flat_mm / (MIN_ARC_SEGMENT_MM))) :
nominal_segments;
const float segment_mm = flat_mm / segments;

// Add hints to help optimize the move
PlannerHints hints;
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
hints.inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
#endif

/**
Expand Down Expand Up @@ -288,6 +291,16 @@ void plan_arc(
int8_t arc_recalc_count = N_ARC_CORRECTION;
#endif

// An arc can always complete within limits from a speed which...
// a) is <= any configured maximum speed,
// b) does not require centripetal force greater than any configured maximum acceleration,
// c) allows the print head to stop in the remining length of the curve within all configured maximum accelerations.
// The last has to be calculated every time through the loop.
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_acceleration_mm_per_s2[axis_q]),
limiting_speed_sqr = _MIN(sq(limiting_speed), limiting_accel * radius);
float arc_mm_remaining = flat_mm;

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

thermalManager.task();
Expand Down Expand Up @@ -342,7 +355,13 @@ void plan_arc(
planner.apply_leveling(raw);
#endif

if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
// calculate safe speed for stopping by the end of the arc
arc_mm_remaining -= segment_mm;

hints.curve_radius = i > 1 ? radius : 0;
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))
break;
}
}
Expand All @@ -363,7 +382,8 @@ void plan_arc(
planner.apply_leveling(raw);
#endif

planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
hints.curve_radius = 0;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);

#if ENABLED(AUTO_BED_LEVELING_UBL)
ARC_LIJKUVW_CODE(
Expand Down
32 changes: 16 additions & 16 deletions Marlin/src/module/motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1041,19 +1041,18 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
NOLESS(segments, 1U);

// The approximate length of each segment
const float inv_segments = 1.0f / float(segments),
cartesian_segment_mm = cartesian_mm * inv_segments;
const float inv_segments = 1.0f / float(segments);
const xyze_float_t segment_distance = diff * inv_segments;

#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm;
#endif
// Add hints to help optimize the move
PlannerHints hints(cartesian_mm * inv_segments);
TERN_(SCARA_FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters);

/*
SERIAL_ECHOPGM("mm=", cartesian_mm);
SERIAL_ECHOPGM(" seconds=", seconds);
SERIAL_ECHOPGM(" segments=", segments);
SERIAL_ECHOPGM(" segment_mm=", cartesian_segment_mm);
SERIAL_ECHOPGM(" segment_mm=", hints.millimeters);
SERIAL_EOL();
//*/

Expand All @@ -1065,11 +1064,12 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
while (--segments) {
segment_idle(next_idle_ms);
raw += segment_distance;
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) break;
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
break;
}

// Ensure last segment arrives at target location.
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, hints);

return false; // caller will update current_position
}
Expand Down Expand Up @@ -1108,17 +1108,16 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
NOLESS(segments, 1U);

// The approximate length of each segment
const float inv_segments = 1.0f / float(segments),
cartesian_segment_mm = cartesian_mm * inv_segments;
const float inv_segments = 1.0f / float(segments);
const xyze_float_t segment_distance = diff * inv_segments;

#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / cartesian_segment_mm;
#endif
// Add hints to help optimize the move
PlannerHints hints(cartesian_mm * inv_segments);
TERN_(SCARA_FEEDRATE_SCALING, hints.inv_duration = scaled_fr_mm_s / hints.millimeters);

//SERIAL_ECHOPGM("mm=", cartesian_mm);
//SERIAL_ECHOLNPGM(" segments=", segments);
//SERIAL_ECHOLNPGM(" segment_mm=", cartesian_segment_mm);
//SERIAL_ECHOLNPGM(" segment_mm=", hints.millimeters);

// Get the raw current position as starting point
xyze_pos_t raw = current_position;
Expand All @@ -1128,12 +1127,13 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
while (--segments) {
segment_idle(next_idle_ms);
raw += segment_distance;
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) break;
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, hints))
break;
}

// Since segment_distance is only approximate,
// the final move must be to the exact destination.
planner.buffer_line(destination, fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
planner.buffer_line(destination, fr_mm_s, active_extruder, hints);
}

#endif // SEGMENT_LEVELED_MOVES && !AUTO_BED_LEVELING_UBL
Expand Down
Loading

0 comments on commit 6bd244a

Please sign in to comment.