|
@@ -113,9 +113,9 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
|
113
|
113
|
float second1 = target[Y_AXIS] + offset[3];
|
114
|
114
|
float t = 0.0;
|
115
|
115
|
|
116
|
|
- float tmp[4];
|
117
|
|
- tmp[X_AXIS] = position[X_AXIS];
|
118
|
|
- tmp[Y_AXIS] = position[Y_AXIS];
|
|
116
|
+ float bez_target[4];
|
|
117
|
+ bez_target[X_AXIS] = position[X_AXIS];
|
|
118
|
+ bez_target[Y_AXIS] = position[Y_AXIS];
|
119
|
119
|
float step = MAX_STEP;
|
120
|
120
|
|
121
|
121
|
uint8_t idle_counter = 0;
|
|
@@ -141,8 +141,8 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
|
141
|
141
|
float candidate_t = 0.5 * (t + new_t);
|
142
|
142
|
float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
|
143
|
143
|
float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
|
144
|
|
- float interp_pos0 = 0.5 * (tmp[X_AXIS] + new_pos0);
|
145
|
|
- float interp_pos1 = 0.5 * (tmp[Y_AXIS] + new_pos1);
|
|
144
|
+ float interp_pos0 = 0.5 * (bez_target[X_AXIS] + new_pos0);
|
|
145
|
+ float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + new_pos1);
|
146
|
146
|
if (dist1(candidate_pos0, candidate_pos1, interp_pos0, interp_pos1) <= (SIGMA)) break;
|
147
|
147
|
new_t = candidate_t;
|
148
|
148
|
new_pos0 = candidate_pos0;
|
|
@@ -157,8 +157,8 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
|
157
|
157
|
if (candidate_t >= 1.0) break;
|
158
|
158
|
float candidate_pos0 = eval_bezier(position[X_AXIS], first0, second0, target[X_AXIS], candidate_t);
|
159
|
159
|
float candidate_pos1 = eval_bezier(position[Y_AXIS], first1, second1, target[Y_AXIS], candidate_t);
|
160
|
|
- float interp_pos0 = 0.5 * (tmp[X_AXIS] + candidate_pos0);
|
161
|
|
- float interp_pos1 = 0.5 * (tmp[Y_AXIS] + candidate_pos1);
|
|
160
|
+ float interp_pos0 = 0.5 * (bez_target[X_AXIS] + candidate_pos0);
|
|
161
|
+ float interp_pos1 = 0.5 * (bez_target[Y_AXIS] + candidate_pos1);
|
162
|
162
|
if (dist1(new_pos0, new_pos1, interp_pos0, interp_pos1) > (SIGMA)) break;
|
163
|
163
|
new_t = candidate_t;
|
164
|
164
|
new_pos0 = candidate_pos0;
|
|
@@ -180,14 +180,23 @@ void cubic_b_spline(const float position[NUM_AXIS], const float target[NUM_AXIS]
|
180
|
180
|
t = new_t;
|
181
|
181
|
|
182
|
182
|
// Compute and send new position
|
183
|
|
- tmp[X_AXIS] = new_pos0;
|
184
|
|
- tmp[Y_AXIS] = new_pos1;
|
|
183
|
+ bez_target[X_AXIS] = new_pos0;
|
|
184
|
+ bez_target[Y_AXIS] = new_pos1;
|
185
|
185
|
// FIXME. The following two are wrong, since the parameter t is
|
186
|
186
|
// not linear in the distance.
|
187
|
|
- tmp[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
|
188
|
|
- tmp[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
|
189
|
|
- clamp_to_software_endstops(tmp);
|
190
|
|
- planner.buffer_line(tmp[X_AXIS], tmp[Y_AXIS], tmp[Z_AXIS], tmp[E_AXIS], feed_rate, extruder);
|
|
187
|
+ bez_target[Z_AXIS] = interp(position[Z_AXIS], target[Z_AXIS], t);
|
|
188
|
+ bez_target[E_AXIS] = interp(position[E_AXIS], target[E_AXIS], t);
|
|
189
|
+ clamp_to_software_endstops(bez_target);
|
|
190
|
+
|
|
191
|
+ #if ENABLED(DELTA) || ENABLED(SCARA)
|
|
192
|
+ calculate_delta(bez_target);
|
|
193
|
+ #if ENABLED(AUTO_BED_LEVELING_FEATURE)
|
|
194
|
+ adjust_delta(bez_target);
|
|
195
|
+ #endif
|
|
196
|
+ planner.buffer_line(delta[X_AXIS], delta[Y_AXIS], delta[Z_AXIS], bez_target[E_AXIS], feed_rate, extruder);
|
|
197
|
+ #else
|
|
198
|
+ planner.buffer_line(bez_target[X_AXIS], bez_target[Y_AXIS], bez_target[Z_AXIS], bez_target[E_AXIS], feed_rate, extruder);
|
|
199
|
+ #endif
|
191
|
200
|
}
|
192
|
201
|
}
|
193
|
202
|
|