|
@@ -29,6 +29,12 @@
|
29
|
29
|
#include "../../module/planner.h"
|
30
|
30
|
#include "../../module/temperature.h"
|
31
|
31
|
|
|
32
|
+#if ENABLED(DELTA)
|
|
33
|
+ #include "../../module/delta.h"
|
|
34
|
+#elif ENABLED(SCARA)
|
|
35
|
+ #include "../../module/scara.h"
|
|
36
|
+#endif
|
|
37
|
+
|
32
|
38
|
#if N_ARC_CORRECTION < 1
|
33
|
39
|
#undef N_ARC_CORRECTION
|
34
|
40
|
#define N_ARC_CORRECTION 1
|
|
@@ -113,7 +119,7 @@ void plan_arc(
|
113
|
119
|
* This is important when there are successive arc motions.
|
114
|
120
|
*/
|
115
|
121
|
// Vector rotation matrix values
|
116
|
|
- float arc_target[XYZE];
|
|
122
|
+ float raw[XYZE];
|
117
|
123
|
const float theta_per_segment = angular_travel / segments,
|
118
|
124
|
linear_per_segment = linear_travel / segments,
|
119
|
125
|
extruder_per_segment = extruder_travel / segments,
|
|
@@ -121,10 +127,10 @@ void plan_arc(
|
121
|
127
|
cos_T = 1 - 0.5 * sq(theta_per_segment); // Small angle approximation
|
122
|
128
|
|
123
|
129
|
// Initialize the linear axis
|
124
|
|
- arc_target[l_axis] = current_position[l_axis];
|
|
130
|
+ raw[l_axis] = current_position[l_axis];
|
125
|
131
|
|
126
|
132
|
// Initialize the extruder axis
|
127
|
|
- arc_target[E_AXIS] = current_position[E_AXIS];
|
|
133
|
+ raw[E_AXIS] = current_position[E_AXIS];
|
128
|
134
|
|
129
|
135
|
const float fr_mm_s = MMS_SCALED(feedrate_mm_s);
|
130
|
136
|
|
|
@@ -134,6 +140,14 @@ void plan_arc(
|
134
|
140
|
int8_t arc_recalc_count = N_ARC_CORRECTION;
|
135
|
141
|
#endif
|
136
|
142
|
|
|
143
|
+ #if ENABLED(SCARA_FEEDRATE_SCALING)
|
|
144
|
+ // SCARA needs to scale the feed rate from mm/s to degrees/s
|
|
145
|
+ const float inv_segment_length = 1.0 / (MM_PER_ARC_SEGMENT),
|
|
146
|
+ inverse_secs = inv_segment_length * fr_mm_s;
|
|
147
|
+ float oldA = stepper.get_axis_position_degrees(A_AXIS),
|
|
148
|
+ oldB = stepper.get_axis_position_degrees(B_AXIS);
|
|
149
|
+ #endif
|
|
150
|
+
|
137
|
151
|
for (uint16_t i = 1; i < segments; i++) { // Iterate (segments-1) times
|
138
|
152
|
|
139
|
153
|
thermalManager.manage_heater();
|
|
@@ -165,19 +179,34 @@ void plan_arc(
|
165
|
179
|
r_Q = -offset[0] * sin_Ti - offset[1] * cos_Ti;
|
166
|
180
|
}
|
167
|
181
|
|
168
|
|
- // Update arc_target location
|
169
|
|
- arc_target[p_axis] = center_P + r_P;
|
170
|
|
- arc_target[q_axis] = center_Q + r_Q;
|
171
|
|
- arc_target[l_axis] += linear_per_segment;
|
172
|
|
- arc_target[E_AXIS] += extruder_per_segment;
|
173
|
|
-
|
174
|
|
- clamp_to_software_endstops(arc_target);
|
175
|
|
-
|
176
|
|
- planner.buffer_line_kinematic(arc_target, fr_mm_s, active_extruder);
|
|
182
|
+ // Update raw location
|
|
183
|
+ raw[p_axis] = center_P + r_P;
|
|
184
|
+ raw[q_axis] = center_Q + r_Q;
|
|
185
|
+ raw[l_axis] += linear_per_segment;
|
|
186
|
+ raw[E_AXIS] += extruder_per_segment;
|
|
187
|
+
|
|
188
|
+ clamp_to_software_endstops(raw);
|
|
189
|
+
|
|
190
|
+ #if ENABLED(SCARA_FEEDRATE_SCALING)
|
|
191
|
+ // For SCARA scale the feed rate from mm/s to degrees/s.
|
|
192
|
+ // i.e., Complete the angular vector in the given time.
|
|
193
|
+ inverse_kinematics(raw);
|
|
194
|
+ ADJUST_DELTA(raw);
|
|
195
|
+ planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], raw[Z_AXIS], raw[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
|
|
196
|
+ oldA = delta[A_AXIS]; oldB = delta[B_AXIS];
|
|
197
|
+ #else
|
|
198
|
+ planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder);
|
|
199
|
+ #endif
|
177
|
200
|
}
|
178
|
201
|
|
179
|
202
|
// Ensure last segment arrives at target location.
|
180
|
|
- planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
|
|
203
|
+ #if ENABLED(SCARA_FEEDRATE_SCALING)
|
|
204
|
+ inverse_kinematics(cart);
|
|
205
|
+ ADJUST_DELTA(cart);
|
|
206
|
+ planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], cart[Z_AXIS], cart[E_AXIS], HYPOT(delta[A_AXIS] - oldA, delta[B_AXIS] - oldB) * inverse_secs, active_extruder);
|
|
207
|
+ #else
|
|
208
|
+ planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder);
|
|
209
|
+ #endif
|
181
|
210
|
|
182
|
211
|
// As far as the parser is concerned, the position is now == target. In reality the
|
183
|
212
|
// motion control system might still be processing the action and the real tool position
|