|
@@ -52,7 +52,8 @@
|
52
|
52
|
void plan_arc(
|
53
|
53
|
const xyze_pos_t &cart, // Destination position
|
54
|
54
|
const ab_float_t &offset, // Center of rotation relative to current_position
|
55
|
|
- const uint8_t clockwise // Clockwise?
|
|
55
|
+ const bool clockwise, // Clockwise?
|
|
56
|
+ const uint8_t circles // Take the scenic route
|
56
|
57
|
) {
|
57
|
58
|
#if ENABLED(CNC_WORKSPACE_PLANES)
|
58
|
59
|
AxisEnum p_axis, q_axis, l_axis;
|
|
@@ -74,9 +75,7 @@ void plan_arc(
|
74
|
75
|
center_Q = current_position[q_axis] - rvec.b,
|
75
|
76
|
rt_X = cart[p_axis] - center_P,
|
76
|
77
|
rt_Y = cart[q_axis] - center_Q,
|
77
|
|
- start_L = current_position[l_axis],
|
78
|
|
- linear_travel = cart[l_axis] - start_L,
|
79
|
|
- extruder_travel = cart.e - current_position.e;
|
|
78
|
+ start_L = current_position[l_axis];
|
80
|
79
|
|
81
|
80
|
// CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required.
|
82
|
81
|
float angular_travel = ATAN2(rvec.a * rt_Y - rvec.b * rt_X, rvec.a * rt_X + rvec.b * rt_Y);
|
|
@@ -90,13 +89,32 @@ void plan_arc(
|
90
|
89
|
if (clockwise) angular_travel -= RADIANS(360);
|
91
|
90
|
|
92
|
91
|
// Make a circle if the angular rotation is 0 and the target is current position
|
93
|
|
- if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) {
|
|
92
|
+ if (NEAR_ZERO(angular_travel) && NEAR(current_position[p_axis], cart[p_axis]) && NEAR(current_position[q_axis], cart[q_axis])) {
|
94
|
93
|
angular_travel = RADIANS(360);
|
95
|
94
|
#ifdef MIN_ARC_SEGMENTS
|
96
|
95
|
min_segments = MIN_ARC_SEGMENTS;
|
97
|
96
|
#endif
|
98
|
97
|
}
|
99
|
98
|
|
|
99
|
+ float linear_travel = cart[l_axis] - start_L,
|
|
100
|
+ extruder_travel = cart.e - current_position.e;
|
|
101
|
+
|
|
102
|
+ // If circling around...
|
|
103
|
+ if (ENABLED(ARC_P_CIRCLES) && circles) {
|
|
104
|
+ const float total_angular = angular_travel + circles * RADIANS(360), // Total rotation with all circles and remainder
|
|
105
|
+ part_per_circle = RADIANS(360) / total_angular, // Each circle's part of the total
|
|
106
|
+ l_per_circle = linear_travel * part_per_circle, // L movement per circle
|
|
107
|
+ e_per_circle = extruder_travel * part_per_circle; // E movement per circle
|
|
108
|
+ xyze_pos_t temp_position = current_position; // for plan_arc to compare to current_position
|
|
109
|
+ for (uint16_t n = circles; n--;) {
|
|
110
|
+ temp_position.e += e_per_circle; // Destination E axis
|
|
111
|
+ temp_position[l_axis] += l_per_circle; // Destination L axis
|
|
112
|
+ plan_arc(temp_position, offset, clockwise, 0); // Plan a single whole circle
|
|
113
|
+ }
|
|
114
|
+ linear_travel = cart[l_axis] - current_position[l_axis];
|
|
115
|
+ extruder_travel = cart.e - current_position.e;
|
|
116
|
+ }
|
|
117
|
+
|
100
|
118
|
const float flat_mm = radius * angular_travel,
|
101
|
119
|
mm_of_travel = linear_travel ? HYPOT(flat_mm, linear_travel) : ABS(flat_mm);
|
102
|
120
|
if (mm_of_travel < 0.001f) return;
|
|
@@ -150,7 +168,7 @@ void plan_arc(
|
150
|
168
|
linear_per_segment = linear_travel / segments,
|
151
|
169
|
extruder_per_segment = extruder_travel / segments,
|
152
|
170
|
sq_theta_per_segment = sq(theta_per_segment),
|
153
|
|
- sin_T = theta_per_segment - sq_theta_per_segment*theta_per_segment/6,
|
|
171
|
+ sin_T = theta_per_segment - sq_theta_per_segment * theta_per_segment / 6,
|
154
|
172
|
cos_T = 1 - 0.5f * sq_theta_per_segment; // Small angle approximation
|
155
|
173
|
|
156
|
174
|
// Initialize the linear axis
|
|
@@ -320,16 +338,15 @@ void GcodeSuite::G2_G3(const bool clockwise) {
|
320
|
338
|
|
321
|
339
|
#if ENABLED(ARC_P_CIRCLES)
|
322
|
340
|
// P indicates number of circles to do
|
323
|
|
- int8_t circles_to_do = parser.byteval('P');
|
|
341
|
+ const int8_t circles_to_do = parser.byteval('P');
|
324
|
342
|
if (!WITHIN(circles_to_do, 0, 100))
|
325
|
343
|
SERIAL_ERROR_MSG(STR_ERR_ARC_ARGS);
|
326
|
|
-
|
327
|
|
- while (circles_to_do--)
|
328
|
|
- plan_arc(current_position, arc_offset, clockwise);
|
|
344
|
+ #else
|
|
345
|
+ constexpr uint8_t circles_to_do = 0;
|
329
|
346
|
#endif
|
330
|
347
|
|
331
|
348
|
// Send the arc to the planner
|
332
|
|
- plan_arc(destination, arc_offset, clockwise);
|
|
349
|
+ plan_arc(destination, arc_offset, clockwise, circles_to_do);
|
333
|
350
|
reset_stepper_timeout();
|
334
|
351
|
}
|
335
|
352
|
else
|