Skip to content

Commit 3311959

Browse files
tombrazierLCh-77
tombrazier
authored and
LCh-77
committed
⚡️ Optimize G2-G3 Arcs (MarlinFirmware#24366)
1 parent e189189 commit 3311959

File tree

8 files changed

+327
-272
lines changed

8 files changed

+327
-272
lines changed

‎Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp

+7-10
Original file line numberDiff line numberDiff line change
@@ -374,11 +374,12 @@
374374
#endif
375375

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

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

384385
xyze_float_t diff = total * inv_segments;
@@ -392,13 +393,9 @@
392393
if (!planner.leveling_active || !planner.leveling_active_at_z(destination.z)) {
393394
while (--segments) {
394395
raw += diff;
395-
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm
396-
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
397-
);
396+
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
398397
}
399-
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, segment_xyz_mm
400-
OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
401-
);
398+
planner.buffer_line(destination, scaled_fr_mm_s, active_extruder, hints);
402399
return false; // Did not set current from destination
403400
}
404401

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

469466
const float oldz = raw.z; raw.z += z_cxcy;
470-
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration) );
467+
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
471468
raw.z = oldz;
472469

473470
if (segments == 0) // done with last segment

‎Marlin/src/feature/joystick.cpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -172,8 +172,9 @@ Joystick joystick;
172172
current_position += move_dist;
173173
apply_motion_limits(current_position);
174174
const float length = sqrt(hypot2);
175+
PlannerHints hints(length);
175176
injecting_now = true;
176-
planner.buffer_line(current_position, length / seg_time, active_extruder, length);
177+
planner.buffer_line(current_position, length / seg_time, active_extruder, hints);
177178
injecting_now = false;
178179
}
179180
}

‎Marlin/src/gcode/motion/G2_G3.cpp

+23-3
Original file line numberDiff line numberDiff line change
@@ -214,9 +214,12 @@ void plan_arc(
214214
const uint16_t segments = nominal_segment_mm > (MAX_ARC_SEGMENT_MM) ? CEIL(flat_mm / (MAX_ARC_SEGMENT_MM)) :
215215
nominal_segment_mm < (MIN_ARC_SEGMENT_MM) ? _MAX(1, FLOOR(flat_mm / (MIN_ARC_SEGMENT_MM))) :
216216
nominal_segments;
217+
const float segment_mm = flat_mm / segments;
217218

219+
// Add hints to help optimize the move
220+
PlannerHints hints;
218221
#if ENABLED(SCARA_FEEDRATE_SCALING)
219-
const float inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
222+
hints.inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
220223
#endif
221224

222225
/**
@@ -288,6 +291,16 @@ void plan_arc(
288291
int8_t arc_recalc_count = N_ARC_CORRECTION;
289292
#endif
290293

294+
// An arc can always complete within limits from a speed which...
295+
// a) is <= any configured maximum speed,
296+
// b) does not require centripetal force greater than any configured maximum acceleration,
297+
// c) allows the print head to stop in the remining length of the curve within all configured maximum accelerations.
298+
// The last has to be calculated every time through the loop.
299+
const float limiting_accel = _MIN(planner.settings.max_acceleration_mm_per_s2[axis_p], planner.settings.max_acceleration_mm_per_s2[axis_q]),
300+
limiting_speed = _MIN(planner.settings.max_feedrate_mm_s[axis_p], planner.settings.max_acceleration_mm_per_s2[axis_q]),
301+
limiting_speed_sqr = _MIN(sq(limiting_speed), limiting_accel * radius);
302+
float arc_mm_remaining = flat_mm;
303+
291304
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
292305

293306
thermalManager.task();
@@ -342,7 +355,13 @@ void plan_arc(
342355
planner.apply_leveling(raw);
343356
#endif
344357

345-
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
358+
// calculate safe speed for stopping by the end of the arc
359+
arc_mm_remaining -= segment_mm;
360+
361+
hints.curve_radius = i > 1 ? radius : 0;
362+
hints.safe_exit_speed_sqr = _MIN(limiting_speed_sqr, 2 * limiting_accel * arc_mm_remaining);
363+
364+
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
346365
break;
347366
}
348367
}
@@ -363,7 +382,8 @@ void plan_arc(
363382
planner.apply_leveling(raw);
364383
#endif
365384

366-
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
385+
hints.curve_radius = 0;
386+
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints);
367387

368388
#if ENABLED(AUTO_BED_LEVELING_UBL)
369389
ARC_LIJKUVW_CODE(

‎Marlin/src/module/motion.cpp

+16-16
Original file line numberDiff line numberDiff line change
@@ -1045,19 +1045,18 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
10451045
NOLESS(segments, 1U);
10461046

10471047
// The approximate length of each segment
1048-
const float inv_segments = 1.0f / float(segments),
1049-
cartesian_segment_mm = cartesian_mm * inv_segments;
1048+
const float inv_segments = 1.0f / float(segments);
10501049
const xyze_float_t segment_distance = diff * inv_segments;
10511050

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

10561055
/*
10571056
SERIAL_ECHOPGM("mm=", cartesian_mm);
10581057
SERIAL_ECHOPGM(" seconds=", seconds);
10591058
SERIAL_ECHOPGM(" segments=", segments);
1060-
SERIAL_ECHOPGM(" segment_mm=", cartesian_segment_mm);
1059+
SERIAL_ECHOPGM(" segment_mm=", hints.millimeters);
10611060
SERIAL_EOL();
10621061
//*/
10631062

@@ -1069,11 +1068,12 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
10691068
while (--segments) {
10701069
segment_idle(next_idle_ms);
10711070
raw += segment_distance;
1072-
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) break;
1071+
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
1072+
break;
10731073
}
10741074

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

10781078
return false; // caller will update current_position
10791079
}
@@ -1112,17 +1112,16 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
11121112
NOLESS(segments, 1U);
11131113

11141114
// The approximate length of each segment
1115-
const float inv_segments = 1.0f / float(segments),
1116-
cartesian_segment_mm = cartesian_mm * inv_segments;
1115+
const float inv_segments = 1.0f / float(segments);
11171116
const xyze_float_t segment_distance = diff * inv_segments;
11181117

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

11231122
//SERIAL_ECHOPGM("mm=", cartesian_mm);
11241123
//SERIAL_ECHOLNPGM(" segments=", segments);
1125-
//SERIAL_ECHOLNPGM(" segment_mm=", cartesian_segment_mm);
1124+
//SERIAL_ECHOLNPGM(" segment_mm=", hints.millimeters);
11261125

11271126
// Get the raw current position as starting point
11281127
xyze_pos_t raw = current_position;
@@ -1132,12 +1131,13 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
11321131
while (--segments) {
11331132
segment_idle(next_idle_ms);
11341133
raw += segment_distance;
1135-
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) break;
1134+
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, hints))
1135+
break;
11361136
}
11371137

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

11431143
#endif // SEGMENT_LEVELED_MOVES && !AUTO_BED_LEVELING_UBL

0 commit comments

Comments
 (0)