From bdf69db0a802ab0f2c6b2bd579ffc1182fabe6c0 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 04:55:02 -0600 Subject: [PATCH] Extend Skew Correction to UBL --- Marlin/SanityCheck.h | 3 --- Marlin/planner.cpp | 18 ++------------ Marlin/planner.h | 24 +++++++++++++++++++ Marlin/ubl_motion.cpp | 56 +++++++++++++++++++++++++------------------ 4 files changed, 59 insertions(+), 42 deletions(-) diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h index e5ddf6b83..75d939f94 100644 --- a/Marlin/SanityCheck.h +++ b/Marlin/SanityCheck.h @@ -1539,9 +1539,6 @@ static_assert(COUNT(sanity_arr_3) <= XYZE_N, "DEFAULT_MAX_ACCELERATION has too m #endif #if ENABLED(SKEW_CORRECTION) - #if ENABLED(AUTO_BED_LEVELING_UBL) && !ENABLED(SEGMENT_LEVELED_MOVES) - #error "SKEW_CORRECTION with AUTO_BED_LEVELING_UBL requires SEGMENT_LEVELED_MOVES." - #endif #if !defined(XY_SKEW_FACTOR) && !(defined(XY_DIAG_AC) && defined(XY_DIAG_BD) && defined(XY_SIDE_AD)) #error "SKEW_CORRECTION requires XY_SKEW_FACTOR or XY_DIAG_AC, XY_DIAG_BD, XY_SIDE_AD." #endif diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index d566982fe..67cf0b391 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -569,14 +569,7 @@ void Planner::calculate_volumetric_multipliers() { void Planner::apply_leveling(float &rx, float &ry, float &rz) { #if ENABLED(SKEW_CORRECTION) - if (WITHIN(rx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(ry, Y_MIN_POS + 1, Y_MAX_POS)) { - const float tempry = ry - (rz * planner.yz_skew_factor), - temprx = rx - (ry * planner.xy_skew_factor) - (rz * (planner.xz_skew_factor - (planner.xy_skew_factor * planner.yz_skew_factor))); - if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) { - rx = temprx; - ry = tempry; - } - } + skew(rx, ry, rz); #endif if (!leveling_active) return; @@ -667,14 +660,7 @@ void Planner::calculate_volumetric_multipliers() { } #if ENABLED(SKEW_CORRECTION) - if (WITHIN(raw[X_AXIS], X_MIN_POS, X_MAX_POS) && WITHIN(raw[Y_AXIS], Y_MIN_POS, Y_MAX_POS)) { - const float temprx = raw[X_AXIS] + raw[Y_AXIS] * planner.xy_skew_factor + raw[Z_AXIS] * planner.xz_skew_factor, - tempry = raw[Y_AXIS] + raw[Z_AXIS] * planner.yz_skew_factor; - if (WITHIN(temprx, X_MIN_POS, X_MAX_POS) && WITHIN(tempry, Y_MIN_POS, Y_MAX_POS)) { - raw[X_AXIS] = temprx; - raw[Y_AXIS] = tempry; - } - } + unskew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); #endif } diff --git a/Marlin/planner.h b/Marlin/planner.h index a30aaacf2..631ccf400 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -341,6 +341,30 @@ class Planner { #endif + #if ENABLED(SKEW_CORRECTION) + + FORCE_INLINE static void skew(float &cx, float &cy, const float &cz) { + if (WITHIN(cx, X_MIN_POS + 1, X_MAX_POS) && WITHIN(cy, Y_MIN_POS + 1, Y_MAX_POS)) { + const float sx = cx - (cy * xy_skew_factor) - (cz * (xz_skew_factor - (xy_skew_factor * yz_skew_factor))), + sy = cy - (cz * yz_skew_factor); + if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) { + cx = sx; cy = sy; + } + } + } + + FORCE_INLINE static void unskew(float &cx, float &cy, const float &cz) { + if (WITHIN(cx, X_MIN_POS, X_MAX_POS) && WITHIN(cy, Y_MIN_POS, Y_MAX_POS)) { + const float sx = cx + cy * xy_skew_factor + cz * xz_skew_factor, + sy = cy + cz * yz_skew_factor; + if (WITHIN(sx, X_MIN_POS, X_MAX_POS) && WITHIN(sy, Y_MIN_POS, Y_MAX_POS)) { + cx = sx; cy = sy; + } + } + } + + #endif // SKEW_CORRECTION + #if PLANNER_LEVELING #define ARG_X float rx diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index 2681cbeb5..880e6d6cb 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -44,18 +44,16 @@ * 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 ENABLED(SKEW_CORRECTION) + // For skew correction just adjust the destination point and we're done + 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] }; + planner.skew(start[X_AXIS], start[Y_AXIS], start[Z_AXIS]); + planner.skew(end[X_AXIS], end[Y_AXIS], end[Z_AXIS]); + #else + const float (&start)[XYZE] = current_position, + (&end)[XYZE] = destination; + #endif const int cell_start_xi = get_cell_index_x(start[X_AXIS]), cell_start_yi = get_cell_index_y(start[Y_AXIS]), @@ -63,13 +61,13 @@ 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_ECHOPAIR(" ubl.line_to_destination_cartesian(xe=", destination[X_AXIS]); + SERIAL_ECHOPAIR(", ye=", destination[Y_AXIS]); + SERIAL_ECHOPAIR(", ze=", destination[Z_AXIS]); + SERIAL_ECHOPAIR(", ee=", destination[E_AXIS]); SERIAL_CHAR(')'); SERIAL_EOL(); - debug_current_and_destination(PSTR("Start of ubl.line_to_destination()")); + debug_current_and_destination(PSTR("Start of ubl.line_to_destination_cartesian()")); } if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) { // if the whole move is within the same cell, @@ -89,7 +87,7 @@ set_current_from_destination(); if (g26_debug_flag) - debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination()")); + debug_current_and_destination(PSTR("out of bounds in ubl.line_to_destination_cartesian()")); return; } @@ -132,7 +130,7 @@ 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()")); + debug_current_and_destination(PSTR("FINAL_MOVE in ubl.line_to_destination_cartesian()")); set_current_from_destination(); return; @@ -238,7 +236,7 @@ } if (g26_debug_flag) - debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination()")); + debug_current_and_destination(PSTR("vertical move done in ubl.line_to_destination_cartesian()")); // // 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. @@ -302,7 +300,7 @@ } if (g26_debug_flag) - debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination()")); + debug_current_and_destination(PSTR("horizontal move done in ubl.line_to_destination_cartesian()")); if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) goto FINAL_MOVE; @@ -396,7 +394,7 @@ } if (g26_debug_flag) - debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination()")); + debug_current_and_destination(PSTR("generic move done in ubl.line_to_destination_cartesian()")); if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS]) goto FINAL_MOVE; @@ -460,9 +458,17 @@ bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&in_target)[XYZE], const float &feedrate) { - if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary + if (!position_is_reachable(in_target[X_AXIS], in_target[Y_AXIS])) // fail if moving outside reachable boundary return true; // did not move, so current_position still accurate + #if ENABLED(SKEW_CORRECTION) + // For skew correction just adjust the destination point and we're done + float rtarget[XYZE] = { in_target[X_AXIS], in_target[Y_AXIS], in_target[Z_AXIS], in_target[E_AXIS] }; + planner.skew(rtarget[X_AXIS], rtarget[Y_AXIS], rtarget[Z_AXIS]); + #else + const float (&rtarget)[XYZE] = in_target; + #endif + const float total[XYZE] = { rtarget[X_AXIS] - current_position[X_AXIS], rtarget[Y_AXIS] - current_position[Y_AXIS], @@ -507,6 +513,10 @@ current_position[E_AXIS] }; + #if ENABLED(SKEW_CORRECTION) + planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); + #endif + // Only compute leveling per segment if ubl active and target below z_fade_height. if (!planner.leveling_active || !planner.leveling_active_at_z(rtarget[Z_AXIS])) { // no mesh leveling while (--segments) {