diff --git a/Marlin/ubl.h b/Marlin/ubl.h index f7c0951ea..754e387dc 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); - - #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]) - ); - } + #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) + + 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 ec731d1fd..2681cbeb5 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -36,193 +36,322 @@ 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] - }; - - 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 !UBL_SEGMENTED - 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] + }; + + 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 (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) { + 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. + */ - // Note: There is no Z Correction in this case. We are off the grid and don't know what - // a reasonable correction would be. + if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) { - planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder); - set_current_from_destination(); + // 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; + } + + FINAL_MOVE: + + /** + * 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 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 + * 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("out of bounds in ubl.line_to_destination()")); + debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()")); + set_current_from_destination(); return; } - FINAL_MOVE: + /** + * 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; /** - * 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. + * 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 float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * (1.0 / (MESH_X_DIST)); + const bool use_x_dist = adx > ady; - 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]); + 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]; - if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0; + const float e_normalized_dist = e_position / on_axis_distance, + z_normalized_dist = z_position / on_axis_distance; - // 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. + int current_xi = cell_start_xi, + current_yi = cell_start_yi; - 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; + 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); /** - * 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. + * 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 (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 (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]; + } - if (g26_debug_flag) - debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination()")); + planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); + } //else printf("FIRST MOVE PRUNED "); + } - set_current_from_destination(); - return; - } + if (g26_debug_flag) + debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()")); - /** - * 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: - */ + // + // 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; - const float dx = end[X_AXIS] - start[X_AXIS], - dy = end[Y_AXIS] - start[Y_AXIS]; + set_current_from_destination(); + return; + } - const int left_flag = dx < 0.0 ? 1 : 0, - down_flag = dy < 0.0 ? 1 : 0; + /** + * + * 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. + * + */ - const float adx = left_flag ? -dx : dx, - ady = down_flag ? -dy : dy; + 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]; + } - 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; + planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); + } //else printf("FIRST MOVE PRUNED "); + } - /** - * 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. - */ + if (g26_debug_flag) + debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()")); - const bool use_x_dist = adx > ady; + if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) + goto FINAL_MOVE; - 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]; + set_current_from_destination(); + return; + } - const float e_normalized_dist = e_position / on_axis_distance, - z_normalized_dist = z_position / on_axis_distance; + /** + * + * This block handles the generic case of a line crossing both X and Y Mesh lines. + * + */ - int current_xi = cell_start_xi, - current_yi = cell_start_yi; + int xi_cnt = cell_start_xi - cell_dest_xi, + yi_cnt = cell_start_yi - cell_dest_yi; - const float m = dy / dx, - c = start[Y_AXIS] - m * start[X_AXIS]; + if (xi_cnt < 0) xi_cnt = -xi_cnt; + if (yi_cnt < 0) yi_cnt = -yi_cnt; - 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); + current_xi += left_flag; + current_yi += down_flag; - /** - * 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; + while (xi_cnt > 0 || yi_cnt > 0) { - 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 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 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 (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]); - const float ry = mesh_index_to_ypos(current_yi); + /** + * 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; } @@ -230,64 +359,27 @@ 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; - 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 + 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 { @@ -295,122 +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("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;