From 3255712343fdc9177cc94c86b2c31bf94ffbb8ca Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sat, 9 Dec 2017 07:50:55 -0600 Subject: [PATCH] Skew Correction for UBL Also remove unused grid slicing function when using UBL segmented. --- Marlin/src/feature/bedlevel/ubl/ubl.h | 2 +- .../src/feature/bedlevel/ubl/ubl_motion.cpp | 47 ++++++++++--------- Marlin/src/module/planner.cpp | 18 +------ Marlin/src/module/planner.h | 24 ++++++++++ 4 files changed, 53 insertions(+), 38 deletions(-) diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h index 1a294d4f75..e7adb083df 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl.h +++ b/Marlin/src/feature/bedlevel/ubl/ubl.h @@ -321,8 +321,8 @@ class unified_bed_leveling { 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, const uint8_t e); + static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate); #define _CMPZ(a,b) (z_values[a][b] == z_values[a][b+1]) #define CMPZ(a) (_CMPZ(a, 0) && _CMPZ(a, 1)) diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp index 942c5dbdbd..63ea07472c 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -47,18 +47,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]), @@ -66,10 +64,10 @@ 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_cartesian()")); @@ -416,12 +414,19 @@ // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, // so we call buffer_segment directly here. Per-segmented leveling and kinematics performed first. - inline void _O2 ubl_buffer_segment_raw(const float (&raw)[XYZE], const float &fr) { + inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) { + + #if ENABLED(SKEW_CORRECTION) + float raw[XYZE] = { in_raw[X_AXIS], in_raw[Y_AXIS], in_raw[Z_AXIS] }; + planner.skew(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); + #else + const float (&raw)[XYZE] = in_raw; + #endif #if ENABLED(DELTA) // apply delta inverse_kinematics DELTA_RAW_IK(); - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder); + planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder); #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) @@ -434,11 +439,11 @@ scara_oldB = delta[B_AXIS]; float s_feedrate = max(adiff, bdiff) * scara_feed_factor; - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder); + planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder); #else // CARTESIAN - planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder); + planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder); #endif } @@ -461,7 +466,7 @@ * Returns true if did NOT move, false if moved (requires current_position update). */ - bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float rtarget[XYZE], const float &feedrate) { + bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) { if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) // fail if moving outside reachable boundary return true; // did not move, so current_position still accurate diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 0d3f9c9562..cf3a316059 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -580,14 +580,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; @@ -678,14 +671,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/src/module/planner.h b/Marlin/src/module/planner.h index 4cdd9cb7ea..6134f57659 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -345,6 +345,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