Skip to content

Commit bfeedda

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

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
@@ -1049,19 +1049,18 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
10491049
NOLESS(segments, 1U);
10501050

10511051
// The approximate length of each segment
1052-
const float inv_segments = 1.0f / float(segments),
1053-
cartesian_segment_mm = cartesian_mm * inv_segments;
1052+
const float inv_segments = 1.0f / float(segments);
10541053
const xyze_float_t segment_distance = diff * inv_segments;
10551054

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

10601059
/*
10611060
SERIAL_ECHOPGM("mm=", cartesian_mm);
10621061
SERIAL_ECHOPGM(" seconds=", seconds);
10631062
SERIAL_ECHOPGM(" segments=", segments);
1064-
SERIAL_ECHOPGM(" segment_mm=", cartesian_segment_mm);
1063+
SERIAL_ECHOPGM(" segment_mm=", hints.millimeters);
10651064
SERIAL_EOL();
10661065
//*/
10671066

@@ -1073,11 +1072,12 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
10731072
while (--segments) {
10741073
segment_idle(next_idle_ms);
10751074
raw += segment_distance;
1076-
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) break;
1075+
if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, hints))
1076+
break;
10771077
}
10781078

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

10821082
return false; // caller will update current_position
10831083
}
@@ -1116,17 +1116,16 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
11161116
NOLESS(segments, 1U);
11171117

11181118
// The approximate length of each segment
1119-
const float inv_segments = 1.0f / float(segments),
1120-
cartesian_segment_mm = cartesian_mm * inv_segments;
1119+
const float inv_segments = 1.0f / float(segments);
11211120
const xyze_float_t segment_distance = diff * inv_segments;
11221121

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

11271126
//SERIAL_ECHOPGM("mm=", cartesian_mm);
11281127
//SERIAL_ECHOLNPGM(" segments=", segments);
1129-
//SERIAL_ECHOLNPGM(" segment_mm=", cartesian_segment_mm);
1128+
//SERIAL_ECHOLNPGM(" segment_mm=", hints.millimeters);
11301129

11311130
// Get the raw current position as starting point
11321131
xyze_pos_t raw = current_position;
@@ -1136,12 +1135,13 @@ FORCE_INLINE void segment_idle(millis_t &next_idle_ms) {
11361135
while (--segments) {
11371136
segment_idle(next_idle_ms);
11381137
raw += segment_distance;
1139-
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, cartesian_segment_mm OPTARG(SCARA_FEEDRATE_SCALING, inv_duration))) break;
1138+
if (!planner.buffer_line(raw, fr_mm_s, active_extruder, hints))
1139+
break;
11401140
}
11411141

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

11471147
#endif // SEGMENT_LEVELED_MOVES && !AUTO_BED_LEVELING_UBL

0 commit comments

Comments
 (0)