|
@@ -75,19 +75,16 @@
|
75
|
75
|
debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()"));
|
76
|
76
|
}
|
77
|
77
|
|
78
|
|
- if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
|
79
|
|
- /**
|
80
|
|
- * we don't need to break up the move
|
81
|
|
- *
|
82
|
|
- * If we are moving off the print bed, we are going to allow the move at this level.
|
83
|
|
- * But we detect it and isolate it. For now, we just pass along the request.
|
84
|
|
- */
|
|
78
|
+ // A move within the same cell needs no splitting
|
|
79
|
+ if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) {
|
85
|
80
|
|
|
81
|
+ // For a move off the bed, use a constant Z raise
|
86
|
82
|
if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
|
87
|
83
|
|
88
|
84
|
// Note: There is no Z Correction in this case. We are off the grid and don't know what
|
89
|
85
|
// a reasonable correction would be. If the user has specified a UBL_Z_RAISE_WHEN_OFF_MESH
|
90
|
86
|
// value, that will be used instead of a calculated (Bi-Linear interpolation) correction.
|
|
87
|
+
|
91
|
88
|
const float z_raise = 0.0
|
92
|
89
|
#ifdef UBL_Z_RAISE_WHEN_OFF_MESH
|
93
|
90
|
+ UBL_Z_RAISE_WHEN_OFF_MESH
|
|
@@ -104,15 +101,7 @@
|
104
|
101
|
|
105
|
102
|
FINAL_MOVE:
|
106
|
103
|
|
107
|
|
- /**
|
108
|
|
- * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
|
109
|
|
- * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
|
110
|
|
- * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
|
111
|
|
- * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
|
112
|
|
- * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
|
113
|
|
- * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
|
114
|
|
- */
|
115
|
|
-
|
|
104
|
+ // The distance is always MESH_X_DIST so multiply by the constant reciprocal.
|
116
|
105
|
const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
|
117
|
106
|
|
118
|
107
|
float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
|
|
@@ -122,22 +111,13 @@
|
122
|
111
|
|
123
|
112
|
if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
|
124
|
113
|
|
125
|
|
- // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
|
126
|
|
- // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
|
127
|
|
-
|
128
|
|
- const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
|
129
|
|
- float z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
|
130
|
|
-
|
131
|
|
- /**
|
132
|
|
- * If part of the Mesh is undefined, it will show up as NAN
|
133
|
|
- * in z_values[][] and propagate through the
|
134
|
|
- * calculations. If our correction is NAN, we throw it out
|
135
|
|
- * because part of the Mesh is undefined and we don't have the
|
136
|
|
- * information we need to complete the height correction.
|
137
|
|
- */
|
138
|
|
- if (isnan(z0)) z0 = 0.0;
|
|
114
|
+ // X cell-fraction done. Interpolate the two Z offsets with the Y fraction for the final Z offset.
|
|
115
|
+ const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST)),
|
|
116
|
+ z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
|
139
|
117
|
|
140
|
|
- planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
|
|
118
|
+ // Undefined parts of the Mesh in z_values[][] are NAN.
|
|
119
|
+ // Replace NAN corrections with 0.0 to prevent NAN propagation.
|
|
120
|
+ planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + (isnan(z0) ? 0.0 : z0), end[E_AXIS], feed_rate, extruder);
|
141
|
121
|
|
142
|
122
|
if (g26_debug_flag)
|
143
|
123
|
debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination_cartesian()"));
|
|
@@ -147,11 +127,8 @@
|
147
|
127
|
}
|
148
|
128
|
|
149
|
129
|
/**
|
150
|
|
- * If we get here, we are processing a move that crosses at least one Mesh Line. We will check
|
151
|
|
- * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
|
152
|
|
- * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
|
153
|
|
- * computation and in fact most lines are of this nature. We will check for that in the following
|
154
|
|
- * blocks of code:
|
|
130
|
+ * Past this point the move is known to cross one or more mesh lines. Check for the most common
|
|
131
|
+ * case - crossing only one X or Y line - after details are worked out to reduce computation.
|
155
|
132
|
*/
|
156
|
133
|
|
157
|
134
|
const float dx = end[X_AXIS] - start[X_AXIS],
|
|
@@ -167,12 +144,11 @@
|
167
|
144
|
dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
|
168
|
145
|
|
169
|
146
|
/**
|
170
|
|
- * Compute the scaling factor for the extruder for each partial move.
|
171
|
|
- * We need to watch out for zero length moves because it will cause us to
|
172
|
|
- * have an infinate scaling factor. We are stuck doing a floating point
|
173
|
|
- * divide to get our scaling factor, but after that, we just multiply by this
|
174
|
|
- * number. We also pick our scaling factor based on whether the X or Y
|
175
|
|
- * component is larger. We use the biggest of the two to preserve precision.
|
|
147
|
+ * Compute the extruder scaling factor for each partial move, checking for
|
|
148
|
+ * zero-length moves that would result in an infinite scaling factor.
|
|
149
|
+ * A float divide is required for this, but then it just multiplies.
|
|
150
|
+ * Also select a scaling factor based on the larger of the X and Y
|
|
151
|
+ * components. The larger of the two is used to preserve precision.
|
176
|
152
|
*/
|
177
|
153
|
|
178
|
154
|
const bool use_x_dist = adx > ady;
|
|
@@ -192,43 +168,37 @@
|
192
|
168
|
|
193
|
169
|
const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
|
194
|
170
|
inf_m_flag = (isinf(m) != 0);
|
|
171
|
+
|
195
|
172
|
/**
|
196
|
|
- * This block handles vertical lines. These are lines that stay within the same
|
197
|
|
- * X Cell column. They do not need to be perfectly vertical. They just can
|
198
|
|
- * not cross into another X Cell column.
|
|
173
|
+ * Handle vertical lines that stay within one column.
|
|
174
|
+ * These need not be perfectly vertical.
|
199
|
175
|
*/
|
200
|
|
- if (dxi == 0) { // Check for a vertical line
|
201
|
|
- current_yi += down_flag; // Line is heading down, we just want to go to the bottom
|
|
176
|
+ if (dxi == 0) { // Vertical line?
|
|
177
|
+ current_yi += down_flag; // Line going down? Just go to the bottom.
|
202
|
178
|
while (current_yi != cell_dest_yi + down_flag) {
|
203
|
179
|
current_yi += dyi;
|
204
|
180
|
const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
|
205
|
181
|
|
206
|
182
|
/**
|
207
|
|
- * if the slope of the line is infinite, we won't do the calculations
|
208
|
|
- * else, we know the next X is the same so we can recover and continue!
|
209
|
|
- * Calculate X at the next Y mesh line
|
|
183
|
+ * Skip the calculations for an infinite slope.
|
|
184
|
+ * For others the next X is the same so this can continue.
|
|
185
|
+ * Calculate X at the next Y mesh line.
|
210
|
186
|
*/
|
211
|
187
|
const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
|
212
|
188
|
|
213
|
189
|
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
|
214
|
190
|
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
|
215
|
191
|
|
216
|
|
- /**
|
217
|
|
- * If part of the Mesh is undefined, it will show up as NAN
|
218
|
|
- * in z_values[][] and propagate through the
|
219
|
|
- * calculations. If our correction is NAN, we throw it out
|
220
|
|
- * because part of the Mesh is undefined and we don't have the
|
221
|
|
- * information we need to complete the height correction.
|
222
|
|
- */
|
|
192
|
+ // Undefined parts of the Mesh in z_values[][] are NAN.
|
|
193
|
+ // Replace NAN corrections with 0.0 to prevent NAN propagation.
|
223
|
194
|
if (isnan(z0)) z0 = 0.0;
|
224
|
195
|
|
225
|
196
|
const float ry = mesh_index_to_ypos(current_yi);
|
226
|
197
|
|
227
|
198
|
/**
|
228
|
|
- * Without this check, it is possible for the algorithm to generate a zero length move in the case
|
229
|
|
- * where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
|
230
|
|
- * happens, it might be best to remove the check and always 'schedule' the move because
|
231
|
|
- * the planner.buffer_segment() routine will filter it if that happens.
|
|
199
|
+ * Without this check, it's possible to generate a zero length move, as in the case where
|
|
200
|
+ * the line is heading down, starting exactly on a mesh line boundary. Since this is rare
|
|
201
|
+ * it might be fine to remove this check and let planner.buffer_segment() filter it out.
|
232
|
202
|
*/
|
233
|
203
|
if (ry != start[Y_AXIS]) {
|
234
|
204
|
if (!inf_normalized_flag) {
|
|
@@ -248,9 +218,7 @@
|
248
|
218
|
if (g26_debug_flag)
|
249
|
219
|
debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination_cartesian()"));
|
250
|
220
|
|
251
|
|
- //
|
252
|
|
- // Check if we are at the final destination. Usually, we won't be, but if it is on a Y Mesh Line, we are done.
|
253
|
|
- //
|
|
221
|
+ // At the final destination? Usually not, but when on a Y Mesh Line it's completed.
|
254
|
222
|
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
|
255
|
223
|
goto FINAL_MOVE;
|
256
|
224
|
|
|
@@ -259,16 +227,11 @@
|
259
|
227
|
}
|
260
|
228
|
|
261
|
229
|
/**
|
262
|
|
- *
|
263
|
|
- * This block handles horizontal lines. These are lines that stay within the same
|
264
|
|
- * Y Cell row. They do not need to be perfectly horizontal. They just can
|
265
|
|
- * not cross into another Y Cell row.
|
266
|
|
- *
|
|
230
|
+ * Handle horizontal lines that stay within one row.
|
|
231
|
+ * These need not be perfectly horizontal.
|
267
|
232
|
*/
|
268
|
|
-
|
269
|
|
- if (dyi == 0) { // Check for a horizontal line
|
270
|
|
- current_xi += left_flag; // Line is heading left, we just want to go to the left
|
271
|
|
- // edge of this cell for the first move.
|
|
233
|
+ if (dyi == 0) { // Horizontal line?
|
|
234
|
+ current_xi += left_flag; // Heading left? Just go to the left edge of the cell for the first move.
|
272
|
235
|
while (current_xi != cell_dest_xi + left_flag) {
|
273
|
236
|
current_xi += dxi;
|
274
|
237
|
const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
|
|
@@ -277,22 +240,16 @@
|
277
|
240
|
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
|
278
|
241
|
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
|
279
|
242
|
|
280
|
|
- /**
|
281
|
|
- * If part of the Mesh is undefined, it will show up as NAN
|
282
|
|
- * in z_values[][] and propagate through the
|
283
|
|
- * calculations. If our correction is NAN, we throw it out
|
284
|
|
- * because part of the Mesh is undefined and we don't have the
|
285
|
|
- * information we need to complete the height correction.
|
286
|
|
- */
|
|
243
|
+ // Undefined parts of the Mesh in z_values[][] are NAN.
|
|
244
|
+ // Replace NAN corrections with 0.0 to prevent NAN propagation.
|
287
|
245
|
if (isnan(z0)) z0 = 0.0;
|
288
|
246
|
|
289
|
247
|
const float rx = mesh_index_to_xpos(current_xi);
|
290
|
248
|
|
291
|
249
|
/**
|
292
|
|
- * Without this check, it is possible for the algorithm to generate a zero length move in the case
|
293
|
|
- * where the line is heading left and it is starting right on a Mesh Line boundary. For how often
|
294
|
|
- * that happens, it might be best to remove the check and always 'schedule' the move because
|
295
|
|
- * the planner.buffer_segment() routine will filter it if that happens.
|
|
250
|
+ * Without this check, it's possible to generate a zero length move, as in the case where
|
|
251
|
+ * the line is heading left, starting exactly on a mesh line boundary. Since this is rare
|
|
252
|
+ * it might be fine to remove this check and let planner.buffer_segment() filter it out.
|
296
|
253
|
*/
|
297
|
254
|
if (rx != start[X_AXIS]) {
|
298
|
255
|
if (!inf_normalized_flag) {
|
|
@@ -321,7 +278,7 @@
|
321
|
278
|
|
322
|
279
|
/**
|
323
|
280
|
*
|
324
|
|
- * This block handles the generic case of a line crossing both X and Y Mesh lines.
|
|
281
|
+ * Handle the generic case of a line crossing both X and Y Mesh lines.
|
325
|
282
|
*
|
326
|
283
|
*/
|
327
|
284
|
|
|
@@ -334,7 +291,7 @@
|
334
|
291
|
current_xi += left_flag;
|
335
|
292
|
current_yi += down_flag;
|
336
|
293
|
|
337
|
|
- while (xi_cnt > 0 || yi_cnt > 0) {
|
|
294
|
+ while (xi_cnt || yi_cnt) {
|
338
|
295
|
|
339
|
296
|
const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
|
340
|
297
|
next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
|
|
@@ -349,13 +306,8 @@
|
349
|
306
|
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
|
350
|
307
|
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
|
351
|
308
|
|
352
|
|
- /**
|
353
|
|
- * If part of the Mesh is undefined, it will show up as NAN
|
354
|
|
- * in z_values[][] and propagate through the
|
355
|
|
- * calculations. If our correction is NAN, we throw it out
|
356
|
|
- * because part of the Mesh is undefined and we don't have the
|
357
|
|
- * information we need to complete the height correction.
|
358
|
|
- */
|
|
309
|
+ // Undefined parts of the Mesh in z_values[][] are NAN.
|
|
310
|
+ // Replace NAN corrections with 0.0 to prevent NAN propagation.
|
359
|
311
|
if (isnan(z0)) z0 = 0.0;
|
360
|
312
|
|
361
|
313
|
if (!inf_normalized_flag) {
|
|
@@ -376,13 +328,8 @@
|
376
|
328
|
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
|
377
|
329
|
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
|
378
|
330
|
|
379
|
|
- /**
|
380
|
|
- * If part of the Mesh is undefined, it will show up as NAN
|
381
|
|
- * in z_values[][] and propagate through the
|
382
|
|
- * calculations. If our correction is NAN, we throw it out
|
383
|
|
- * because part of the Mesh is undefined and we don't have the
|
384
|
|
- * information we need to complete the height correction.
|
385
|
|
- */
|
|
331
|
+ // Undefined parts of the Mesh in z_values[][] are NAN.
|
|
332
|
+ // Replace NAN corrections with 0.0 to prevent NAN propagation.
|
386
|
333
|
if (isnan(z0)) z0 = 0.0;
|
387
|
334
|
|
388
|
335
|
if (!inf_normalized_flag) {
|
|
@@ -400,7 +347,7 @@
|
400
|
347
|
xi_cnt--;
|
401
|
348
|
}
|
402
|
349
|
|
403
|
|
- if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
|
|
350
|
+ //if (xi_cnt < 0 || yi_cnt < 0) break; // Too far! Exit the loop and go to FINAL_MOVE
|
404
|
351
|
}
|
405
|
352
|
|
406
|
353
|
if (g26_debug_flag)
|