Skip to content

Commit 6bd244a

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

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

10431043
// The approximate length of each segment
1044-
const float inv_segments = 1.0f / float(segments),
1045-
cartesian_segment_mm = cartesian_mm * inv_segments;
1044+
const float inv_segments = 1.0f / float(segments);
10461045
const xyze_float_t segment_distance = diff * inv_segments;
10471046

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

10521051
/*
10531052
SERIAL_ECHOPGM("mm=", cartesian_mm);
10541053
SERIAL_ECHOPGM(" seconds=", seconds);
10551054
SERIAL_ECHOPGM(" segments=", segments);
1056-
SERIAL_ECHOPGM(" segment_mm=", cartesian_segment_mm);
1055+
SERIAL_ECHOPGM(" segment_mm=", hints.millimeters);
10571056
SERIAL_EOL();
10581057
//*/
10591058

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

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

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

11101110
// The approximate length of each segment
1111-
const float inv_segments = 1.0f / float(segments),
1112-
cartesian_segment_mm = cartesian_mm * inv_segments;
1111+
const float inv_segments = 1.0f / float(segments);
11131112
const xyze_float_t segment_distance = diff * inv_segments;
11141113

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

11191118
//SERIAL_ECHOPGM("mm=", cartesian_mm);
11201119
//SERIAL_ECHOLNPGM(" segments=", segments);
1121-
//SERIAL_ECHOLNPGM(" segment_mm=", cartesian_segment_mm);
1120+
//SERIAL_ECHOLNPGM(" segment_mm=", hints.millimeters);
11221121

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

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

11391139
#endif // SEGMENT_LEVELED_MOVES && !AUTO_BED_LEVELING_UBL

0 commit comments

Comments
 (0)