|
@@ -39,14 +39,21 @@
|
39
|
39
|
#undef N_ARC_CORRECTION
|
40
|
40
|
#define N_ARC_CORRECTION 1
|
41
|
41
|
#endif
|
|
42
|
+#ifndef MIN_CIRCLE_SEGMENTS
|
|
43
|
+ #define MIN_CIRCLE_SEGMENTS 72 // 5° per segment
|
|
44
|
+#endif
|
|
45
|
+#if !defined(MAX_ARC_SEGMENT_MM) && defined(MIN_ARC_SEGMENT_MM)
|
|
46
|
+ #define MAX_ARC_SEGMENT_MM MIN_ARC_SEGMENT_MM
|
|
47
|
+#elif !defined(MIN_ARC_SEGMENT_MM) && defined(MAX_ARC_SEGMENT_MM)
|
|
48
|
+ #define MIN_ARC_SEGMENT_MM MAX_ARC_SEGMENT_MM
|
|
49
|
+#endif
|
|
50
|
+
|
|
51
|
+#define ARC_LIJK_CODE(L,I,J,K) CODE_N(SUB2(LINEAR_AXES),L,I,J,K)
|
|
52
|
+#define ARC_LIJKE_CODE(L,I,J,K,E) ARC_LIJK_CODE(L,I,J,K); CODE_ITEM_E(E)
|
42
|
53
|
|
43
|
54
|
/**
|
44
|
|
- * Plan an arc in 2 dimensions, with optional linear motion in a 3rd dimension
|
45
|
|
- *
|
46
|
|
- * The arc is traced by generating many small linear segments, as configured by
|
47
|
|
- * MM_PER_ARC_SEGMENT (Default 1mm). In the future we hope more slicers will include
|
48
|
|
- * an option to generate G2/G3 arcs for curved surfaces, as this will allow faster
|
49
|
|
- * boards to produce much smoother curved surfaces.
|
|
55
|
+ * Plan an arc in 2 dimensions, with linear motion in the other axes.
|
|
56
|
+ * The arc is traced with many small linear segments according to the configuration.
|
50
|
57
|
*/
|
51
|
58
|
void plan_arc(
|
52
|
59
|
const xyze_pos_t &cart, // Destination position
|
|
@@ -55,41 +62,45 @@ void plan_arc(
|
55
|
62
|
const uint8_t circles // Take the scenic route
|
56
|
63
|
) {
|
57
|
64
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
58
|
|
- AxisEnum p_axis, q_axis, l_axis;
|
|
65
|
+ AxisEnum axis_p, axis_q, axis_l;
|
59
|
66
|
switch (gcode.workspace_plane) {
|
60
|
67
|
default:
|
61
|
|
- case GcodeSuite::PLANE_XY: p_axis = X_AXIS; q_axis = Y_AXIS; l_axis = Z_AXIS; break;
|
62
|
|
- case GcodeSuite::PLANE_YZ: p_axis = Y_AXIS; q_axis = Z_AXIS; l_axis = X_AXIS; break;
|
63
|
|
- case GcodeSuite::PLANE_ZX: p_axis = Z_AXIS; q_axis = X_AXIS; l_axis = Y_AXIS; break;
|
|
68
|
+ case GcodeSuite::PLANE_XY: axis_p = X_AXIS; axis_q = Y_AXIS; axis_l = Z_AXIS; break;
|
|
69
|
+ case GcodeSuite::PLANE_YZ: axis_p = Y_AXIS; axis_q = Z_AXIS; axis_l = X_AXIS; break;
|
|
70
|
+ case GcodeSuite::PLANE_ZX: axis_p = Z_AXIS; axis_q = X_AXIS; axis_l = Y_AXIS; break;
|
64
|
71
|
}
|
65
|
72
|
#else
|
66
|
|
- constexpr AxisEnum p_axis = X_AXIS, q_axis = Y_AXIS OPTARG(HAS_Z_AXIS, l_axis = Z_AXIS);
|
|
73
|
+ constexpr AxisEnum axis_p = X_AXIS, axis_q = Y_AXIS OPTARG(HAS_Z_AXIS, axis_l = Z_AXIS);
|
67
|
74
|
#endif
|
68
|
75
|
|
69
|
76
|
// Radius vector from center to current location
|
70
|
77
|
ab_float_t rvec = -offset;
|
71
|
78
|
|
72
|
79
|
const float radius = HYPOT(rvec.a, rvec.b),
|
73
|
|
- center_P = current_position[p_axis] - rvec.a,
|
74
|
|
- center_Q = current_position[q_axis] - rvec.b,
|
75
|
|
- rt_X = cart[p_axis] - center_P,
|
76
|
|
- rt_Y = cart[q_axis] - center_Q
|
77
|
|
- OPTARG(HAS_Z_AXIS, start_L = current_position[l_axis]);
|
78
|
|
-
|
79
|
|
- #ifdef MIN_ARC_SEGMENTS
|
80
|
|
- uint16_t min_segments = MIN_ARC_SEGMENTS;
|
81
|
|
- #else
|
82
|
|
- constexpr uint16_t min_segments = 1;
|
83
|
|
- #endif
|
|
80
|
+ center_P = current_position[axis_p] - rvec.a,
|
|
81
|
+ center_Q = current_position[axis_q] - rvec.b,
|
|
82
|
+ rt_X = cart[axis_p] - center_P,
|
|
83
|
+ rt_Y = cart[axis_q] - center_Q;
|
|
84
|
+
|
|
85
|
+ ARC_LIJK_CODE(
|
|
86
|
+ const float start_L = current_position[axis_l],
|
|
87
|
+ const float start_I = current_position.i,
|
|
88
|
+ const float start_J = current_position.j,
|
|
89
|
+ const float start_K = current_position.k
|
|
90
|
+ );
|
84
|
91
|
|
85
|
92
|
// Angle of rotation between position and target from the circle center.
|
86
|
93
|
float angular_travel, abs_angular_travel;
|
87
|
94
|
|
|
95
|
+ // Minimum number of segments in an arc move
|
|
96
|
+ uint16_t min_segments = 1;
|
|
97
|
+
|
88
|
98
|
// Do a full circle if starting and ending positions are "identical"
|
89
|
|
- if (NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
|
|
99
|
+ if (NEAR(current_position[axis_p], cart[axis_p]) && NEAR(current_position[axis_q], cart[axis_q])) {
|
90
|
100
|
// Preserve direction for circles
|
91
|
101
|
angular_travel = clockwise ? -RADIANS(360) : RADIANS(360);
|
92
|
102
|
abs_angular_travel = RADIANS(360);
|
|
103
|
+ min_segments = MIN_CIRCLE_SEGMENTS;
|
93
|
104
|
}
|
94
|
105
|
else {
|
95
|
106
|
// Calculate the angle
|
|
@@ -106,61 +117,90 @@ void plan_arc(
|
106
|
117
|
|
107
|
118
|
abs_angular_travel = ABS(angular_travel);
|
108
|
119
|
|
109
|
|
- #ifdef MIN_ARC_SEGMENTS
|
110
|
|
- min_segments = CEIL(min_segments * abs_angular_travel / RADIANS(360));
|
111
|
|
- NOLESS(min_segments, 1U);
|
112
|
|
- #endif
|
|
120
|
+ // Apply minimum segments to the arc
|
|
121
|
+ const float portion_of_circle = abs_angular_travel / RADIANS(360); // Portion of a complete circle (0 < N < 1)
|
|
122
|
+ min_segments = CEIL((MIN_CIRCLE_SEGMENTS) * portion_of_circle); // Minimum segments for the arc
|
113
|
123
|
}
|
114
|
124
|
|
115
|
|
- #if HAS_Z_AXIS
|
116
|
|
- float linear_travel = cart[l_axis] - start_L;
|
117
|
|
- #endif
|
118
|
|
- #if HAS_EXTRUDERS
|
119
|
|
- float extruder_travel = cart.e - current_position.e;
|
120
|
|
- #endif
|
|
125
|
+ ARC_LIJKE_CODE(
|
|
126
|
+ float travel_L = cart[axis_l] - start_L,
|
|
127
|
+ float travel_I = cart.i - start_I,
|
|
128
|
+ float travel_J = cart.j - start_J,
|
|
129
|
+ float travel_K = cart.k - start_K,
|
|
130
|
+ float travel_E = cart.e - current_position.e
|
|
131
|
+ );
|
121
|
132
|
|
122
|
|
- // If circling around...
|
|
133
|
+ // If "P" specified circles, call plan_arc recursively then continue with the rest of the arc
|
123
|
134
|
if (TERN0(ARC_P_CIRCLES, circles)) {
|
124
|
|
- const float total_angular = abs_angular_travel + circles * RADIANS(360), // Total rotation with all circles and remainder
|
125
|
|
- part_per_circle = RADIANS(360) / total_angular; // Each circle's part of the total
|
126
|
|
-
|
127
|
|
- #if HAS_Z_AXIS
|
128
|
|
- const float l_per_circle = linear_travel * part_per_circle; // L movement per circle
|
129
|
|
- #endif
|
130
|
|
- #if HAS_EXTRUDERS
|
131
|
|
- const float e_per_circle = extruder_travel * part_per_circle; // E movement per circle
|
132
|
|
- #endif
|
133
|
|
-
|
134
|
|
- xyze_pos_t temp_position = current_position; // for plan_arc to compare to current_position
|
|
135
|
+ const float total_angular = abs_angular_travel + circles * RADIANS(360), // Total rotation with all circles and remainder
|
|
136
|
+ part_per_circle = RADIANS(360) / total_angular; // Each circle's part of the total
|
|
137
|
+
|
|
138
|
+ ARC_LIJKE_CODE(
|
|
139
|
+ const float per_circle_L = travel_L * part_per_circle, // L movement per circle
|
|
140
|
+ const float per_circle_I = travel_I * part_per_circle,
|
|
141
|
+ const float per_circle_J = travel_J * part_per_circle,
|
|
142
|
+ const float per_circle_K = travel_K * part_per_circle,
|
|
143
|
+ const float per_circle_E = travel_E * part_per_circle // E movement per circle
|
|
144
|
+ );
|
|
145
|
+
|
|
146
|
+ xyze_pos_t temp_position = current_position;
|
135
|
147
|
for (uint16_t n = circles; n--;) {
|
136
|
|
- TERN_(HAS_EXTRUDERS, temp_position.e += e_per_circle); // Destination E axis
|
137
|
|
- TERN_(HAS_Z_AXIS, temp_position[l_axis] += l_per_circle); // Destination L axis
|
138
|
|
- plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle
|
|
148
|
+ ARC_LIJKE_CODE( // Destination Linear Axes
|
|
149
|
+ temp_position[axis_l] += per_circle_L,
|
|
150
|
+ temp_position.i += per_circle_I,
|
|
151
|
+ temp_position.j += per_circle_J,
|
|
152
|
+ temp_position.k += per_circle_K,
|
|
153
|
+ temp_position.e += per_circle_E // Destination E axis
|
|
154
|
+ );
|
|
155
|
+ plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle
|
139
|
156
|
}
|
140
|
|
- TERN_(HAS_Z_AXIS, linear_travel = cart[l_axis] - current_position[l_axis]);
|
141
|
|
- TERN_(HAS_EXTRUDERS, extruder_travel = cart.e - current_position.e);
|
|
157
|
+ ARC_LIJKE_CODE(
|
|
158
|
+ travel_L = cart[axis_l] - current_position[axis_l],
|
|
159
|
+ travel_I = cart.i - current_position.i,
|
|
160
|
+ travel_J = cart.j - current_position.j,
|
|
161
|
+ travel_K = cart.k - current_position.k,
|
|
162
|
+ travel_E = cart.e - current_position.e
|
|
163
|
+ );
|
142
|
164
|
}
|
143
|
165
|
|
144
|
|
- const float flat_mm = radius * abs_angular_travel,
|
145
|
|
- mm_of_travel = TERN_(HAS_Z_AXIS, linear_travel ? HYPOT(flat_mm, linear_travel) :) flat_mm;
|
146
|
|
- if (mm_of_travel < 0.001f) return;
|
|
166
|
+ // Millimeters in the arc, assuming it's flat
|
|
167
|
+ const float flat_mm = radius * abs_angular_travel;
|
147
|
168
|
|
|
169
|
+ // Return if the move is near zero
|
|
170
|
+ if (flat_mm < 0.0001f
|
|
171
|
+ GANG_N(SUB2(LINEAR_AXES),
|
|
172
|
+ && travel_L < 0.0001f,
|
|
173
|
+ && travel_I < 0.0001f,
|
|
174
|
+ && travel_J < 0.0001f,
|
|
175
|
+ && travel_K < 0.0001f
|
|
176
|
+ )
|
|
177
|
+ ) return;
|
|
178
|
+
|
|
179
|
+ // Feedrate for the move, scaled by the feedrate multiplier
|
148
|
180
|
const feedRate_t scaled_fr_mm_s = MMS_SCALED(feedrate_mm_s);
|
149
|
181
|
|
150
|
|
- // Start with a nominal segment length
|
151
|
|
- float seg_length = (
|
152
|
|
- #ifdef ARC_SEGMENTS_PER_R
|
153
|
|
- constrain(MM_PER_ARC_SEGMENT * radius, MM_PER_ARC_SEGMENT, ARC_SEGMENTS_PER_R)
|
154
|
|
- #elif ARC_SEGMENTS_PER_SEC
|
155
|
|
- _MAX(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MM_PER_ARC_SEGMENT)
|
|
182
|
+ // Get the nominal segment length based on settings
|
|
183
|
+ const float nominal_segment_mm = (
|
|
184
|
+ #if ARC_SEGMENTS_PER_SEC // Length based on segments per second and feedrate
|
|
185
|
+ constrain(scaled_fr_mm_s * RECIPROCAL(ARC_SEGMENTS_PER_SEC), MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM)
|
156
|
186
|
#else
|
157
|
|
- MM_PER_ARC_SEGMENT
|
|
187
|
+ MAX_ARC_SEGMENT_MM // Length using the maximum segment size
|
158
|
188
|
#endif
|
159
|
189
|
);
|
160
|
|
- // Divide total travel by nominal segment length
|
161
|
|
- uint16_t segments = FLOOR(mm_of_travel / seg_length);
|
162
|
|
- NOLESS(segments, min_segments); // At least some segments
|
163
|
|
- seg_length = mm_of_travel / segments;
|
|
190
|
+
|
|
191
|
+ // Number of whole segments based on the nominal segment length
|
|
192
|
+ const float nominal_segments = _MAX(FLOOR(flat_mm / nominal_segment_mm), min_segments);
|
|
193
|
+
|
|
194
|
+ // A new segment length based on the required minimum
|
|
195
|
+ const float segment_mm = constrain(flat_mm / nominal_segments, MIN_ARC_SEGMENT_MM, MAX_ARC_SEGMENT_MM);
|
|
196
|
+
|
|
197
|
+ // The number of whole segments in the arc, ignoring the remainder
|
|
198
|
+ uint16_t segments = FLOOR(flat_mm / segment_mm);
|
|
199
|
+
|
|
200
|
+ // Are the segments now too few to reach the destination?
|
|
201
|
+ const float segmented_length = segment_mm * segments;
|
|
202
|
+ const bool tooshort = segmented_length < flat_mm - 0.0001f;
|
|
203
|
+ const float proportion = tooshort ? segmented_length / flat_mm : 1.0f;
|
164
|
204
|
|
165
|
205
|
/**
|
166
|
206
|
* Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector,
|
|
@@ -190,26 +230,36 @@ void plan_arc(
|
190
|
230
|
*/
|
191
|
231
|
// Vector rotation matrix values
|
192
|
232
|
xyze_pos_t raw;
|
193
|
|
- const float theta_per_segment = angular_travel / segments,
|
|
233
|
+ const float theta_per_segment = proportion * angular_travel / segments,
|
194
|
234
|
sq_theta_per_segment = sq(theta_per_segment),
|
195
|
235
|
sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
|
196
|
236
|
cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
|
197
|
237
|
|
198
|
|
- #if HAS_Z_AXIS && DISABLED(AUTO_BED_LEVELING_UBL)
|
199
|
|
- const float linear_per_segment = linear_travel / segments;
|
200
|
|
- #endif
|
201
|
|
- #if HAS_EXTRUDERS
|
202
|
|
- const float extruder_per_segment = extruder_travel / segments;
|
|
238
|
+ #if DISABLED(AUTO_BED_LEVELING_UBL)
|
|
239
|
+ ARC_LIJK_CODE(
|
|
240
|
+ const float per_segment_L = proportion * travel_L / segments,
|
|
241
|
+ const float per_segment_I = proportion * travel_I / segments,
|
|
242
|
+ const float per_segment_J = proportion * travel_J / segments,
|
|
243
|
+ const float per_segment_K = proportion * travel_K / segments
|
|
244
|
+ );
|
203
|
245
|
#endif
|
204
|
246
|
|
205
|
|
- // Initialize the linear axis
|
206
|
|
- TERN_(HAS_Z_AXIS, raw[l_axis] = current_position[l_axis]);
|
|
247
|
+ CODE_ITEM_E(const float extruder_per_segment = proportion * travel_E / segments);
|
207
|
248
|
|
208
|
|
- // Initialize the extruder axis
|
209
|
|
- TERN_(HAS_EXTRUDERS, raw.e = current_position.e);
|
|
249
|
+ // For shortened segments, run all but the remainder in the loop
|
|
250
|
+ if (tooshort) segments++;
|
|
251
|
+
|
|
252
|
+ // Initialize all linear axes and E
|
|
253
|
+ ARC_LIJKE_CODE(
|
|
254
|
+ raw[axis_l] = current_position[axis_l],
|
|
255
|
+ raw.i = current_position.i,
|
|
256
|
+ raw.j = current_position.j,
|
|
257
|
+ raw.k = current_position.k,
|
|
258
|
+ raw.e = current_position.e
|
|
259
|
+ );
|
210
|
260
|
|
211
|
261
|
#if ENABLED(SCARA_FEEDRATE_SCALING)
|
212
|
|
- const float inv_duration = scaled_fr_mm_s / seg_length;
|
|
262
|
+ const float inv_duration = scaled_fr_mm_s / segment_mm;
|
213
|
263
|
#endif
|
214
|
264
|
|
215
|
265
|
millis_t next_idle_ms = millis() + 200UL;
|
|
@@ -221,8 +271,9 @@ void plan_arc(
|
221
|
271
|
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
|
222
|
272
|
|
223
|
273
|
thermalManager.manage_heater();
|
224
|
|
- if (ELAPSED(millis(), next_idle_ms)) {
|
225
|
|
- next_idle_ms = millis() + 200UL;
|
|
274
|
+ const millis_t ms = millis();
|
|
275
|
+ if (ELAPSED(ms, next_idle_ms)) {
|
|
276
|
+ next_idle_ms = ms + 200UL;
|
226
|
277
|
idle();
|
227
|
278
|
}
|
228
|
279
|
|
|
@@ -250,13 +301,16 @@ void plan_arc(
|
250
|
301
|
}
|
251
|
302
|
|
252
|
303
|
// Update raw location
|
253
|
|
- raw[p_axis] = center_P + rvec.a;
|
254
|
|
- raw[q_axis] = center_Q + rvec.b;
|
255
|
|
- #if HAS_Z_AXIS
|
256
|
|
- raw[l_axis] = TERN(AUTO_BED_LEVELING_UBL, start_L, raw[l_axis] + linear_per_segment);
|
257
|
|
- #endif
|
258
|
|
-
|
259
|
|
- TERN_(HAS_EXTRUDERS, raw.e += extruder_per_segment);
|
|
304
|
+ raw[axis_p] = center_P + rvec.a;
|
|
305
|
+ raw[axis_q] = center_Q + rvec.b;
|
|
306
|
+ ARC_LIJKE_CODE(
|
|
307
|
+ #if ENABLED(AUTO_BED_LEVELING_UBL)
|
|
308
|
+ raw[axis_l] = start_L, raw.i = start_I, raw.j = start_J, raw.k = start_K
|
|
309
|
+ #else
|
|
310
|
+ raw[axis_l] += per_segment_L, raw.i += per_segment_I, raw.j += per_segment_J, raw.k += per_segment_K
|
|
311
|
+ #endif
|
|
312
|
+ , raw.e += extruder_per_segment
|
|
313
|
+ );
|
260
|
314
|
|
261
|
315
|
apply_motion_limits(raw);
|
262
|
316
|
|
|
@@ -264,14 +318,15 @@ void plan_arc(
|
264
|
318
|
planner.apply_leveling(raw);
|
265
|
319
|
#endif
|
266
|
320
|
|
267
|
|
- if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0
|
268
|
|
- OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
|
269
|
|
- )) break;
|
|
321
|
+ if (!planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)))
|
|
322
|
+ break;
|
270
|
323
|
}
|
271
|
324
|
|
272
|
325
|
// Ensure last segment arrives at target location.
|
273
|
326
|
raw = cart;
|
274
|
|
- TERN_(AUTO_BED_LEVELING_UBL, TERN_(HAS_Z_AXIS, raw[l_axis] = start_L));
|
|
327
|
+ #if ENABLED(AUTO_BED_LEVELING_UBL)
|
|
328
|
+ ARC_LIJK_CODE(raw[axis_l] = start_L, raw.i = start_I, raw.j = start_J, raw.k = start_K);
|
|
329
|
+ #endif
|
275
|
330
|
|
276
|
331
|
apply_motion_limits(raw);
|
277
|
332
|
|
|
@@ -279,11 +334,11 @@ void plan_arc(
|
279
|
334
|
planner.apply_leveling(raw);
|
280
|
335
|
#endif
|
281
|
336
|
|
282
|
|
- planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0
|
283
|
|
- OPTARG(SCARA_FEEDRATE_SCALING, inv_duration)
|
284
|
|
- );
|
|
337
|
+ planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, 0 OPTARG(SCARA_FEEDRATE_SCALING, inv_duration));
|
285
|
338
|
|
286
|
|
- TERN_(AUTO_BED_LEVELING_UBL, TERN_(HAS_Z_AXIS, raw[l_axis] = start_L));
|
|
339
|
+ #if ENABLED(AUTO_BED_LEVELING_UBL)
|
|
340
|
+ ARC_LIJK_CODE(raw[axis_l] = start_L, raw.i = start_I, raw.j = start_J, raw.k = start_K);
|
|
341
|
+ #endif
|
287
|
342
|
current_position = raw;
|
288
|
343
|
|
289
|
344
|
} // plan_arc
|
|
@@ -325,7 +380,7 @@ void GcodeSuite::G2_G3(const bool clockwise) {
|
325
|
380
|
relative_mode = true;
|
326
|
381
|
#endif
|
327
|
382
|
|
328
|
|
- get_destination_from_command(); // Get X Y Z E F (and set cutter power)
|
|
383
|
+ get_destination_from_command(); // Get X Y [Z[I[J[K]]]] [E] F (and set cutter power)
|
329
|
384
|
|
330
|
385
|
TERN_(SF_ARC_FIX, relative_mode = relative_mode_backup);
|
331
|
386
|
|