From 5cce532a29658024eafe417ca2e9d96165458b7f Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Sat, 9 Dec 2017 03:34:41 -0600
Subject: [PATCH] One or the other?
---
Marlin/ubl.h | 29 +-
Marlin/ubl_motion.cpp | 650 +++++++++++++++++++++---------------------
2 files changed, 338 insertions(+), 341 deletions(-)
diff --git a/Marlin/ubl.h b/Marlin/ubl.h
index f7c0951ea9f..754e387dc39 100644
--- a/Marlin/ubl.h
+++ b/Marlin/ubl.h
@@ -326,21 +326,24 @@
return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
}
- static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate);
- static void line_to_destination_cartesian(const float &fr, uint8_t e);
+ #if UBL_SEGMENTED
+ static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate);
+ #else
+ static void line_to_destination_cartesian(const float &fr, const uint8_t e);
+ #endif
- #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
- #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))
- #define ZZER(a) (z_values[a][0] == 0)
+ #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1])
+ #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1))
+ #define ZZER(a) (z_values[a][0] == 0)
- FORCE_INLINE bool mesh_is_valid() {
- return !(
- ( CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal?
- && ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge?
- )
- || isnan(z_values[0][0])
- );
- }
+ FORCE_INLINE bool mesh_is_valid() {
+ return !(
+ ( CMPZ(0) && CMPZ(1) && CMPZ(2) // adjacent z values all equal?
+ && ZZER(0) && ZZER(1) && ZZER(2) // all zero at the edge?
+ )
+ || isnan(z_values[0][0])
+ );
+ }
}; // class unified_bed_leveling
extern unified_bed_leveling ubl;
diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp
index ec731d1fdbf..2681cbeb56b 100644
--- a/Marlin/ubl_motion.cpp
+++ b/Marlin/ubl_motion.cpp
@@ -36,172 +36,89 @@
extern void set_current_from_destination();
#endif
- void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) {
- /**
- * Much of the nozzle movement will be within the same cell. So we will do as little computation
- * as possible to determine if this is the case. If this move is within the same cell, we will
- * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
- */
- const float start[XYZE] = {
- current_position[X_AXIS],
- current_position[Y_AXIS],
- current_position[Z_AXIS],
- current_position[E_AXIS]
- },
- end[XYZE] = {
- destination[X_AXIS],
- destination[Y_AXIS],
- destination[Z_AXIS],
- destination[E_AXIS]
- };
+ #if !UBL_SEGMENTED
- const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
- cell_start_yi = get_cell_index_y(start[Y_AXIS]),
- cell_dest_xi = get_cell_index_x(end[X_AXIS]),
- cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
-
- if (g26_debug_flag) {
- SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
- SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
- SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
- SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
- SERIAL_CHAR(')');
- SERIAL_EOL();
- debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
- }
-
- if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
+ void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const uint8_t extruder) {
/**
- * we don't need to break up the move
- *
- * If we are moving off the print bed, we are going to allow the move at this level.
- * But we detect it and isolate it. For now, we just pass along the request.
+ * Much of the nozzle movement will be within the same cell. So we will do as little computation
+ * as possible to determine if this is the case. If this move is within the same cell, we will
+ * just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/
+ const float start[XYZE] = {
+ current_position[X_AXIS],
+ current_position[Y_AXIS],
+ current_position[Z_AXIS],
+ current_position[E_AXIS]
+ },
+ end[XYZE] = {
+ destination[X_AXIS],
+ destination[Y_AXIS],
+ destination[Z_AXIS],
+ destination[E_AXIS]
+ };
- if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
+ const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
+ cell_start_yi = get_cell_index_y(start[Y_AXIS]),
+ cell_dest_xi = get_cell_index_x(end[X_AXIS]),
+ cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
- // Note: There is no Z Correction in this case. We are off the grid and don't know what
- // a reasonable correction would be.
-
- planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
- set_current_from_destination();
-
- if (g26_debug_flag)
- debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
-
- return;
+ if (g26_debug_flag) {
+ SERIAL_ECHOPAIR(" ubl.line_to_destination(xe=", end[X_AXIS]);
+ SERIAL_ECHOPAIR(", ye=", end[Y_AXIS]);
+ SERIAL_ECHOPAIR(", ze=", end[Z_AXIS]);
+ SERIAL_ECHOPAIR(", ee=", end[E_AXIS]);
+ SERIAL_CHAR(')');
+ SERIAL_EOL();
+ debug_current_and_destination(PSTR("Start of ubl.line_to_destination()"));
}
- FINAL_MOVE:
+ if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell,
+ /**
+ * we don't need to break up the move
+ *
+ * If we are moving off the print bed, we are going to allow the move at this level.
+ * But we detect it and isolate it. For now, we just pass along the request.
+ */
- /**
- * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
- * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
- * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
- * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
- * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
- * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
- */
+ if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
- const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
+ // Note: There is no Z Correction in this case. We are off the grid and don't know what
+ // a reasonable correction would be.
- float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
- (z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]),
- z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio *
- (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
+ planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder);
+ set_current_from_destination();
- if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
+ if (g26_debug_flag)
+ debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()"));
- // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
- // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
+ return;
+ }
- const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
- 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;
-
- /**
- * If part of the Mesh is undefined, it will show up as NAN
- * in z_values[][] and propagate through the
- * calculations. If our correction is NAN, we throw it out
- * because part of the Mesh is undefined and we don't have the
- * information we need to complete the height correction.
- */
- if (isnan(z0)) z0 = 0.0;
-
- planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
-
- if (g26_debug_flag)
- debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
-
- set_current_from_destination();
- return;
- }
-
- /**
- * If we get here, we are processing a move that crosses at least one Mesh Line. We will check
- * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
- * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
- * computation and in fact most lines are of this nature. We will check for that in the following
- * blocks of code:
- */
-
- const float dx = end[X_AXIS] - start[X_AXIS],
- dy = end[Y_AXIS] - start[Y_AXIS];
-
- const int left_flag = dx < 0.0 ? 1 : 0,
- down_flag = dy < 0.0 ? 1 : 0;
-
- const float adx = left_flag ? -dx : dx,
- ady = down_flag ? -dy : dy;
-
- const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
- dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
-
- /**
- * Compute the scaling factor for the extruder for each partial move.
- * We need to watch out for zero length moves because it will cause us to
- * have an infinate scaling factor. We are stuck doing a floating point
- * divide to get our scaling factor, but after that, we just multiply by this
- * number. We also pick our scaling factor based on whether the X or Y
- * component is larger. We use the biggest of the two to preserve precision.
- */
-
- const bool use_x_dist = adx > ady;
-
- float on_axis_distance = use_x_dist ? dx : dy,
- e_position = end[E_AXIS] - start[E_AXIS],
- z_position = end[Z_AXIS] - start[Z_AXIS];
-
- const float e_normalized_dist = e_position / on_axis_distance,
- z_normalized_dist = z_position / on_axis_distance;
-
- int current_xi = cell_start_xi,
- current_yi = cell_start_yi;
-
- const float m = dy / dx,
- c = start[Y_AXIS] - m * start[X_AXIS];
-
- const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
- inf_m_flag = (isinf(m) != 0);
- /**
- * This block handles vertical lines. These are lines that stay within the same
- * X Cell column. They do not need to be perfectly vertical. They just can
- * not cross into another X Cell column.
- */
- if (dxi == 0) { // Check for a vertical line
- current_yi += down_flag; // Line is heading down, we just want to go to the bottom
- while (current_yi != cell_dest_yi + down_flag) {
- current_yi += dyi;
- const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
+ FINAL_MOVE:
/**
- * if the slope of the line is infinite, we won't do the calculations
- * else, we know the next X is the same so we can recover and continue!
- * Calculate X at the next Y mesh line
+ * Optimize some floating point operations here. We could call float get_z_correction(float x0, float y0) to
+ * generate the correction for us. But we can lighten the load on the CPU by doing a modified version of the function.
+ * We are going to only calculate the amount we are from the first mesh line towards the second mesh line once.
+ * We will use this fraction in both of the original two Z Height calculations for the bi-linear interpolation. And,
+ * instead of doing a generic divide of the distance, we know the distance is MESH_X_DIST so we can use the preprocessor
+ * to create a 1-over number for us. That will allow us to do a floating point multiply instead of a floating point divide.
*/
- const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
- float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
- * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+ const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST));
+
+ float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
+ (z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]),
+ z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio *
+ (z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
+
+ if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
+
+ // we are done with the fractional X distance into the cell. Now with the two Z-Heights we have calculated, we
+ // are going to apply the Y-Distance into the cell to interpolate the final Z correction.
+
+ const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * (1.0 / (MESH_Y_DIST));
+ 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;
/**
* If part of the Mesh is undefined, it will show up as NAN
@@ -212,17 +129,256 @@
*/
if (isnan(z0)) z0 = 0.0;
- const float ry = mesh_index_to_ypos(current_yi);
+ planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder);
+
+ if (g26_debug_flag)
+ debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()"));
+
+ set_current_from_destination();
+ return;
+ }
+
+ /**
+ * If we get here, we are processing a move that crosses at least one Mesh Line. We will check
+ * for the simple case of just crossing X or just crossing Y Mesh Lines after we get all the details
+ * of the move figured out. We can process the easy case of just crossing an X or Y Mesh Line with less
+ * computation and in fact most lines are of this nature. We will check for that in the following
+ * blocks of code:
+ */
+
+ const float dx = end[X_AXIS] - start[X_AXIS],
+ dy = end[Y_AXIS] - start[Y_AXIS];
+
+ const int left_flag = dx < 0.0 ? 1 : 0,
+ down_flag = dy < 0.0 ? 1 : 0;
+
+ const float adx = left_flag ? -dx : dx,
+ ady = down_flag ? -dy : dy;
+
+ const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
+ dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
+
+ /**
+ * Compute the scaling factor for the extruder for each partial move.
+ * We need to watch out for zero length moves because it will cause us to
+ * have an infinate scaling factor. We are stuck doing a floating point
+ * divide to get our scaling factor, but after that, we just multiply by this
+ * number. We also pick our scaling factor based on whether the X or Y
+ * component is larger. We use the biggest of the two to preserve precision.
+ */
+
+ const bool use_x_dist = adx > ady;
+
+ float on_axis_distance = use_x_dist ? dx : dy,
+ e_position = end[E_AXIS] - start[E_AXIS],
+ z_position = end[Z_AXIS] - start[Z_AXIS];
+
+ const float e_normalized_dist = e_position / on_axis_distance,
+ z_normalized_dist = z_position / on_axis_distance;
+
+ int current_xi = cell_start_xi,
+ current_yi = cell_start_yi;
+
+ const float m = dy / dx,
+ c = start[Y_AXIS] - m * start[X_AXIS];
+
+ const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
+ inf_m_flag = (isinf(m) != 0);
+ /**
+ * This block handles vertical lines. These are lines that stay within the same
+ * X Cell column. They do not need to be perfectly vertical. They just can
+ * not cross into another X Cell column.
+ */
+ if (dxi == 0) { // Check for a vertical line
+ current_yi += down_flag; // Line is heading down, we just want to go to the bottom
+ while (current_yi != cell_dest_yi + down_flag) {
+ current_yi += dyi;
+ const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
+
+ /**
+ * if the slope of the line is infinite, we won't do the calculations
+ * else, we know the next X is the same so we can recover and continue!
+ * Calculate X at the next Y mesh line
+ */
+ const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
+
+ float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
+ * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+ /**
+ * If part of the Mesh is undefined, it will show up as NAN
+ * in z_values[][] and propagate through the
+ * calculations. If our correction is NAN, we throw it out
+ * because part of the Mesh is undefined and we don't have the
+ * information we need to complete the height correction.
+ */
+ if (isnan(z0)) z0 = 0.0;
+
+ const float ry = mesh_index_to_ypos(current_yi);
+
+ /**
+ * Without this check, it is possible for the algorithm to generate a zero length move in the case
+ * where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
+ * happens, it might be best to remove the check and always 'schedule' the move because
+ * the planner.buffer_segment() routine will filter it if that happens.
+ */
+ if (ry != start[Y_AXIS]) {
+ if (!inf_normalized_flag) {
+ on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+ e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
+ z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
+ }
+ else {
+ e_position = end[E_AXIS];
+ z_position = end[Z_AXIS];
+ }
+
+ planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
+ } //else printf("FIRST MOVE PRUNED ");
+ }
+
+ if (g26_debug_flag)
+ debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
+
+ //
+ // 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.
+ //
+ if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
+ goto FINAL_MOVE;
+
+ set_current_from_destination();
+ return;
+ }
+
+ /**
+ *
+ * This block handles horizontal lines. These are lines that stay within the same
+ * Y Cell row. They do not need to be perfectly horizontal. They just can
+ * not cross into another Y Cell row.
+ *
+ */
+
+ if (dyi == 0) { // Check for a horizontal line
+ current_xi += left_flag; // Line is heading left, we just want to go to the left
+ // edge of this cell for the first move.
+ while (current_xi != cell_dest_xi + left_flag) {
+ current_xi += dxi;
+ const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
+ ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
+
+ float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
+ * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+ /**
+ * If part of the Mesh is undefined, it will show up as NAN
+ * in z_values[][] and propagate through the
+ * calculations. If our correction is NAN, we throw it out
+ * because part of the Mesh is undefined and we don't have the
+ * information we need to complete the height correction.
+ */
+ if (isnan(z0)) z0 = 0.0;
+
+ const float rx = mesh_index_to_xpos(current_xi);
+
+ /**
+ * Without this check, it is possible for the algorithm to generate a zero length move in the case
+ * where the line is heading left and it is starting right on a Mesh Line boundary. For how often
+ * that happens, it might be best to remove the check and always 'schedule' the move because
+ * the planner.buffer_segment() routine will filter it if that happens.
+ */
+ if (rx != start[X_AXIS]) {
+ if (!inf_normalized_flag) {
+ on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+ e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
+ z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
+ }
+ else {
+ e_position = end[E_AXIS];
+ z_position = end[Z_AXIS];
+ }
+
+ planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
+ } //else printf("FIRST MOVE PRUNED ");
+ }
+
+ if (g26_debug_flag)
+ debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
+
+ if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
+ goto FINAL_MOVE;
+
+ set_current_from_destination();
+ return;
+ }
+
+ /**
+ *
+ * This block handles the generic case of a line crossing both X and Y Mesh lines.
+ *
+ */
+
+ int xi_cnt = cell_start_xi - cell_dest_xi,
+ yi_cnt = cell_start_yi - cell_dest_yi;
+
+ if (xi_cnt < 0) xi_cnt = -xi_cnt;
+ if (yi_cnt < 0) yi_cnt = -yi_cnt;
+
+ current_xi += left_flag;
+ current_yi += down_flag;
+
+ while (xi_cnt > 0 || yi_cnt > 0) {
+
+ const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
+ next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
+ ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
+ rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
+ // (No need to worry about m being zero.
+ // If that was the case, it was already detected
+ // as a vertical line move above.)
+
+ if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
+ // Yes! Crossing a Y Mesh Line next
+ float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
+ * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+ /**
+ * If part of the Mesh is undefined, it will show up as NAN
+ * in z_values[][] and propagate through the
+ * calculations. If our correction is NAN, we throw it out
+ * because part of the Mesh is undefined and we don't have the
+ * information we need to complete the height correction.
+ */
+ if (isnan(z0)) z0 = 0.0;
- /**
- * Without this check, it is possible for the algorithm to generate a zero length move in the case
- * where the line is heading down and it is starting right on a Mesh Line boundary. For how often that
- * happens, it might be best to remove the check and always 'schedule' the move because
- * the planner.buffer_segment() routine will filter it if that happens.
- */
- if (ry != start[Y_AXIS]) {
if (!inf_normalized_flag) {
- on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
+ on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
+ e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
+ z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
+ }
+ else {
+ e_position = end[E_AXIS];
+ z_position = end[Z_AXIS];
+ }
+ planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
+ current_yi += dyi;
+ yi_cnt--;
+ }
+ else {
+ // Yes! Crossing a X Mesh Line next
+ float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
+ * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
+
+ /**
+ * If part of the Mesh is undefined, it will show up as NAN
+ * in z_values[][] and propagate through the
+ * calculations. If our correction is NAN, we throw it out
+ * because part of the Mesh is undefined and we don't have the
+ * information we need to complete the height correction.
+ */
+ if (isnan(z0)) z0 = 0.0;
+
+ if (!inf_normalized_flag) {
+ on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
}
@@ -231,186 +387,24 @@
z_position = end[Z_AXIS];
}
- planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
- } //else printf("FIRST MOVE PRUNED ");
+ planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
+ current_xi += dxi;
+ xi_cnt--;
+ }
+
+ if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
}
if (g26_debug_flag)
- debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()"));
-
- //
- // 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.
- //
- if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
- goto FINAL_MOVE;
-
- set_current_from_destination();
- return;
- }
-
- /**
- *
- * This block handles horizontal lines. These are lines that stay within the same
- * Y Cell row. They do not need to be perfectly horizontal. They just can
- * not cross into another Y Cell row.
- *
- */
-
- if (dyi == 0) { // Check for a horizontal line
- current_xi += left_flag; // Line is heading left, we just want to go to the left
- // edge of this cell for the first move.
- while (current_xi != cell_dest_xi + left_flag) {
- current_xi += dxi;
- const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
- ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
-
- float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
- * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
- /**
- * If part of the Mesh is undefined, it will show up as NAN
- * in z_values[][] and propagate through the
- * calculations. If our correction is NAN, we throw it out
- * because part of the Mesh is undefined and we don't have the
- * information we need to complete the height correction.
- */
- if (isnan(z0)) z0 = 0.0;
-
- const float rx = mesh_index_to_xpos(current_xi);
-
- /**
- * Without this check, it is possible for the algorithm to generate a zero length move in the case
- * where the line is heading left and it is starting right on a Mesh Line boundary. For how often
- * that happens, it might be best to remove the check and always 'schedule' the move because
- * the planner.buffer_segment() routine will filter it if that happens.
- */
- if (rx != start[X_AXIS]) {
- if (!inf_normalized_flag) {
- on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS];
- e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
- z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
- }
- else {
- e_position = end[E_AXIS];
- z_position = end[Z_AXIS];
- }
-
- planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder);
- } //else printf("FIRST MOVE PRUNED ");
- }
-
- if (g26_debug_flag)
- debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()"));
+ debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
goto FINAL_MOVE;
set_current_from_destination();
- return;
}
- /**
- *
- * This block handles the generic case of a line crossing both X and Y Mesh lines.
- *
- */
-
- int xi_cnt = cell_start_xi - cell_dest_xi,
- yi_cnt = cell_start_yi - cell_dest_yi;
-
- if (xi_cnt < 0) xi_cnt = -xi_cnt;
- if (yi_cnt < 0) yi_cnt = -yi_cnt;
-
- current_xi += left_flag;
- current_yi += down_flag;
-
- while (xi_cnt > 0 || yi_cnt > 0) {
-
- const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
- next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
- ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line
- rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line
- // (No need to worry about m being zero.
- // If that was the case, it was already detected
- // as a vertical line move above.)
-
- if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
- // Yes! Crossing a Y Mesh Line next
- float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi)
- * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
- /**
- * If part of the Mesh is undefined, it will show up as NAN
- * in z_values[][] and propagate through the
- * calculations. If our correction is NAN, we throw it out
- * because part of the Mesh is undefined and we don't have the
- * information we need to complete the height correction.
- */
- if (isnan(z0)) z0 = 0.0;
-
- if (!inf_normalized_flag) {
- on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS];
- e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
- z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
- }
- else {
- e_position = end[E_AXIS];
- z_position = end[Z_AXIS];
- }
- planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder);
- current_yi += dyi;
- yi_cnt--;
- }
- else {
- // Yes! Crossing a X Mesh Line next
- float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag)
- * planner.fade_scaling_factor_for_z(end[Z_AXIS]);
-
- /**
- * If part of the Mesh is undefined, it will show up as NAN
- * in z_values[][] and propagate through the
- * calculations. If our correction is NAN, we throw it out
- * because part of the Mesh is undefined and we don't have the
- * information we need to complete the height correction.
- */
- if (isnan(z0)) z0 = 0.0;
-
- if (!inf_normalized_flag) {
- on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS];
- e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist;
- z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist;
- }
- else {
- e_position = end[E_AXIS];
- z_position = end[Z_AXIS];
- }
-
- planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder);
- current_xi += dxi;
- xi_cnt--;
- }
-
- if (xi_cnt < 0 || yi_cnt < 0) break; // we've gone too far, so exit the loop and move on to FINAL_MOVE
- }
-
- if (g26_debug_flag)
- debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()"));
-
- if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
- goto FINAL_MOVE;
-
- set_current_from_destination();
- }
-
- #if UBL_SEGMENTED
-
- // macro to inline copy exactly 4 floats, don't rely on sizeof operator
- #define COPY_XYZE( target, source ) { \
- target[X_AXIS] = source[X_AXIS]; \
- target[Y_AXIS] = source[Y_AXIS]; \
- target[Z_AXIS] = source[Z_AXIS]; \
- target[E_AXIS] = source[E_AXIS]; \
- }
+ #else // UBL_SEGMENTED
#if IS_SCARA // scale the feed rate from mm/s to degrees/s
static float scara_feed_factor, scara_oldA, scara_oldB;