|
@@ -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.
|