Browse Source

🐛 Fix G2/G3 Arcs stutter / JD speed (#24362)

tombrazier 3 years ago
parent
commit
1a6a604310
No account linked to committer's email address
1 changed files with 97 additions and 100 deletions
  1. 97
    100
      Marlin/src/gcode/motion/G2_G3.cpp

+ 97
- 100
Marlin/src/gcode/motion/G2_G3.cpp View File

@@ -197,8 +197,8 @@ void plan_arc(
197 197
   // Feedrate for the move, scaled by the feedrate multiplier
198 198
   const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
199 199
 
200
-  // Get the nominal segment length based on settings
201
-  const float nominal_segment_mm = (
200
+  // Get the ideal segment length for the move based on settings
201
+  const float ideal_segment_mm = (
202 202
     #if ARC_SEGMENTS_PER_SEC  // Length based on segments per second and feedrate
203 203
       constrain(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM)
204 204
     #else
@@ -206,19 +206,18 @@ void plan_arc(
206 206
     #endif
207 207
   );
208 208
 
209
-  // Number of whole segments based on the nominal segment length
210
-  const float nominal_segments = _MAX(FLOOR(flat_mm / nominal_segment_mm), min_segments);
209
+  // Number of whole segments based on the ideal segment length
210
+  const float nominal_segments = _MAX(FLOOR(flat_mm / ideal_segment_mm), min_segments),
211
+              nominal_segment_mm = flat_mm / nominal_segments;
211 212
 
212
-  // A new segment length based on the required minimum
213
-  const float segment_mm = constrain(flat_mm / nominal_segments, MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM);
213
+  // The number of whole segments in the arc, with best attempt to honor MIN_ARC_SEGMENT_MM and MAX_ARC_SEGMENT_MM
214
+  const uint16_t segments = nominal_segment_mm > (MAX_ARC_SEGMENT_MM) ? CEIL(flat_mm / (MAX_ARC_SEGMENT_MM)) :
215
+                            nominal_segment_mm < (MIN_ARC_SEGMENT_MM) ? _MAX(1, FLOOR(flat_mm / (MIN_ARC_SEGMENT_MM))) :
216
+                            nominal_segments;
214 217
 
215
-  // The number of whole segments in the arc, ignoring the remainder
216
-  uint16_t segments = FLOOR(flat_mm / segment_mm);
217
-
218
-  // Are the segments now too few to reach the destination?
219
-  const float segmented_length = segment_mm * segments;
220
-  const bool tooshort = segmented_length < flat_mm - 0.0001f;
221
-  const float proportion = tooshort ? segmented_length / flat_mm : 1.0f;
218
+  #if ENABLED(SCARA_FEEDRATE_SCALING)
219
+    const float inv_duration = (scaled_fr_mm_s / flat_mm) * segments;
220
+  #endif
222 221
 
223 222
   /**
224 223
    * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
@@ -246,108 +245,106 @@ void plan_arc(
246 245
    * a correction, the planner should have caught up to the lag caused by the initial plan_arc overhead.
247 246
    * This is important when there are successive arc motions.
248 247
    */
249
-  // Vector rotation matrix values
250
-  xyze_pos_t raw;
251
-  const float theta_per_segment = proportion * angular_travel / segments,
252
-              sq_theta_per_segment = sq(theta_per_segment),
253
-              sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
254
-              cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
255
-
256
-  #if DISABLED(AUTO_BED_LEVELING_UBL)
257
-    ARC_LIJKUVW_CODE(
258
-      const float per_segment_L = proportion * travel_L / segments,
259
-      const float per_segment_I = proportion * travel_I / segments,
260
-      const float per_segment_J = proportion * travel_J / segments,
261
-      const float per_segment_K = proportion * travel_K / segments,
262
-      const float per_segment_U = proportion * travel_U / segments,
263
-      const float per_segment_V = proportion * travel_V / segments,
264
-      const float per_segment_W = proportion * travel_W / segments
265
-    );
266
-  #endif
267 248
 
268
-  CODE_ITEM_E(const float extruder_per_segment = proportion * travel_E / segments);
269
-
270
-  // For shortened segments, run all but the remainder in the loop
271
-  if (tooshort) segments++;
249
+  xyze_pos_t raw;
272 250
 
273
-  // Initialize all linear axes and E
274
-  ARC_LIJKUVWE_CODE(
275
-    raw[axis_l] = current_position[axis_l],
276
-    raw.i       = current_position.i,
277
-    raw.j       = current_position.j,
278
-    raw.k       = current_position.k,
279
-    raw.u       = current_position.u,
280
-    raw.v       = current_position.v,
281
-    raw.w       = current_position.w,
282
-    raw.e       = current_position.e
283
-  );
251
+  // do not calculate rotation parameters for trivial single-segment arcs
252
+  if (segments > 1) {
253
+    // Vector rotation matrix values
254
+    const float theta_per_segment = angular_travel / segments,
255
+                sq_theta_per_segment = sq(theta_per_segment),
256
+                sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
257
+                cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
258
+
259
+    #if DISABLED(AUTO_BED_LEVELING_UBL)
260
+      ARC_LIJKUVW_CODE(
261
+        const float per_segment_L = travel_L / segments,
262
+        const float per_segment_I = travel_I / segments,
263
+        const float per_segment_J = travel_J / segments,
264
+        const float per_segment_K = travel_K / segments,
265
+        const float per_segment_U = travel_U / segments,
266
+        const float per_segment_V = travel_V / segments,
267
+        const float per_segment_W = travel_W / segments
268
+      );
269
+    #endif
284 270
 
285
-  #if ENABLED(SCARA_FEEDRATE_SCALING)
286
-    const float inv_duration = scaled_fr_mm_s / segment_mm;
287
-  #endif
271
+    CODE_ITEM_E(const float extruder_per_segment = travel_E / segments);
288 272
 
289
-  millis_t next_idle_ms = millis() + 200UL;
273
+    // Initialize all linear axes and E
274
+    ARC_LIJKUVWE_CODE(
275
+      raw[axis_l] = current_position[axis_l],
276
+      raw.i       = current_position.i,
277
+      raw.j       = current_position.j,
278
+      raw.k       = current_position.k,
279
+      raw.u       = current_position.u,
280
+      raw.v       = current_position.v,
281
+      raw.w       = current_position.w,
282
+      raw.e       = current_position.e
283
+    );
290 284
 
291
-  #if N_ARC_CORRECTION > 1
292
-    int8_t arc_recalc_count = N_ARC_CORRECTION;
293
-  #endif
285
+    millis_t next_idle_ms = millis() + 200UL;
294 286
 
295
-  for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
287
+    #if N_ARC_CORRECTION > 1
288
+      int8_t arc_recalc_count = N_ARC_CORRECTION;
289
+    #endif
296 290
 
297
-    thermalManager.manage_heater();
298
-    const millis_t ms = millis();
299
-    if (ELAPSED(ms, next_idle_ms)) {
300
-      next_idle_ms = ms + 200UL;
301
-      idle();
302
-    }
291
+    for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
303 292
 
304
-    #if N_ARC_CORRECTION > 1
305
-      if (--arc_recalc_count) {
306
-        // Apply vector rotation matrix to previous rvec.a / 1
307
-        const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
308
-        rvec.a = rvec.a * cos_T - rvec.b * sin_T;
309
-        rvec.b = r_new_Y;
293
+      thermalManager.manage_heater();
294
+      const millis_t ms = millis();
295
+      if (ELAPSED(ms, next_idle_ms)) {
296
+        next_idle_ms = ms + 200UL;
297
+        idle();
310 298
       }
311
-      else
312
-    #endif
313
-    {
299
+
314 300
       #if N_ARC_CORRECTION > 1
315
-        arc_recalc_count = N_ARC_CORRECTION;
301
+        if (--arc_recalc_count) {
302
+          // Apply vector rotation matrix to previous rvec.a / 1
303
+          const float r_new_Y = rvec.a * sin_T + rvec.b * cos_T;
304
+          rvec.a = rvec.a * cos_T - rvec.b * sin_T;
305
+          rvec.b = r_new_Y;
306
+        }
307
+        else
316 308
       #endif
309
+      {
310
+        #if N_ARC_CORRECTION > 1
311
+          arc_recalc_count = N_ARC_CORRECTION;
312
+        #endif
313
+
314
+        // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
315
+        // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
316
+        // To reduce stuttering, the sin and cos could be computed at different times.
317
+        // For now, compute both at the same time.
318
+        const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
319
+        rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
320
+        rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
321
+      }
317 322
 
318
-      // Arc correction to radius vector. Computed only every N_ARC_CORRECTION increments.
319
-      // Compute exact location by applying transformation matrix from initial radius vector(=-offset).
320
-      // To reduce stuttering, the sin and cos could be computed at different times.
321
-      // For now, compute both at the same time.
322
-      const float cos_Ti = cos(i * theta_per_segment), sin_Ti = sin(i * theta_per_segment);
323
-      rvec.a = -offset[0] * cos_Ti + offset[1] * sin_Ti;
324
-      rvec.b = -offset[0] * sin_Ti - offset[1] * cos_Ti;
325
-    }
326
-
327
-    // Update raw location
328
-    raw[axis_p] = center_P + rvec.a;
329
-    raw[axis_q] = center_Q + rvec.b;
330
-    ARC_LIJKUVWE_CODE(
331
-      #if ENABLED(AUTO_BED_LEVELING_UBL)
332
-        raw[axis_l] = start_L,
333
-        raw.i = start_I, raw.j = start_J, raw.k = start_K,
334
-        raw.u = start_U, raw.v = start_V, raw.w = start_V
335
-      #else
336
-        raw[axis_l] += per_segment_L,
337
-        raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K,
338
-        raw.u += per_segment_U, raw.v += per_segment_V, raw.w += per_segment_W
339
-      #endif
340
-      , raw.e += extruder_per_segment
341
-    );
323
+      // Update raw location
324
+      raw[axis_p] = center_P + rvec.a;
325
+      raw[axis_q] = center_Q + rvec.b;
326
+      ARC_LIJKUVWE_CODE(
327
+        #if ENABLED(AUTO_BED_LEVELING_UBL)
328
+          raw[axis_l] = start_L,
329
+          raw.i = start_I, raw.j = start_J, raw.k = start_K,
330
+          raw.u = start_U, raw.v = start_V, raw.w = start_V
331
+        #else
332
+          raw[axis_l] += per_segment_L,
333
+          raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K,
334
+          raw.u += per_segment_U, raw.v += per_segment_V, raw.w += per_segment_W
335
+        #endif
336
+        , raw.e += extruder_per_segment
337
+      );
342 338
 
343
-    apply_motion_limits(raw);
339
+      apply_motion_limits(raw);
344 340
 
345
-    #if HAS_LEVELING && !PLANNER_LEVELING
346
-      planner.apply_leveling(raw);
347
-    #endif
341
+      #if HAS_LEVELING && !PLANNER_LEVELING
342
+        planner.apply_leveling(raw);
343
+      #endif
348 344
 
349
-    if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
350
-      break;
345
+      if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
346
+        break;
347
+    }
351 348
   }
352 349
 
353 350
   // Ensure last segment arrives at target location.

Loading…
Cancel
Save