diff options
Diffstat (limited to 'Marlin/src/feature/bedlevel/abl')
| -rw-r--r-- | Marlin/src/feature/bedlevel/abl/abl.cpp | 421 | ||||
| -rw-r--r-- | Marlin/src/feature/bedlevel/abl/abl.h | 45 |
2 files changed, 466 insertions, 0 deletions
diff --git a/Marlin/src/feature/bedlevel/abl/abl.cpp b/Marlin/src/feature/bedlevel/abl/abl.cpp new file mode 100644 index 0000000..3fb0cfc --- /dev/null +++ b/Marlin/src/feature/bedlevel/abl/abl.cpp @@ -0,0 +1,421 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(AUTO_BED_LEVELING_BILINEAR) + +#include "../bedlevel.h" + +#include "../../../module/motion.h" + +#define DEBUG_OUT ENABLED(DEBUG_LEVELING_FEATURE) +#include "../../../core/debug_out.h" + +#if ENABLED(EXTENSIBLE_UI) + #include "../../../lcd/extui/ui_api.h" +#endif + +xy_pos_t bilinear_grid_spacing, bilinear_start; +xy_float_t bilinear_grid_factor; +bed_mesh_t z_values; + +/** + * Extrapolate a single point from its neighbors + */ +static void extrapolate_one_point(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) { + if (!isnan(z_values[x][y])) return; + if (DEBUGGING(LEVELING)) { + DEBUG_ECHOPGM("Extrapolate ["); + if (x < 10) DEBUG_CHAR(' '); + DEBUG_ECHO((int)x); + DEBUG_CHAR(xdir ? (xdir > 0 ? '+' : '-') : ' '); + DEBUG_CHAR(' '); + if (y < 10) DEBUG_CHAR(' '); + DEBUG_ECHO((int)y); + DEBUG_CHAR(ydir ? (ydir > 0 ? '+' : '-') : ' '); + DEBUG_ECHOLNPGM("]"); + } + + // Get X neighbors, Y neighbors, and XY neighbors + const uint8_t x1 = x + xdir, y1 = y + ydir, x2 = x1 + xdir, y2 = y1 + ydir; + float a1 = z_values[x1][y ], a2 = z_values[x2][y ], + b1 = z_values[x ][y1], b2 = z_values[x ][y2], + c1 = z_values[x1][y1], c2 = z_values[x2][y2]; + + // Treat far unprobed points as zero, near as equal to far + if (isnan(a2)) a2 = 0.0; + if (isnan(a1)) a1 = a2; + if (isnan(b2)) b2 = 0.0; + if (isnan(b1)) b1 = b2; + if (isnan(c2)) c2 = 0.0; + if (isnan(c1)) c1 = c2; + + const float a = 2 * a1 - a2, b = 2 * b1 - b2, c = 2 * c1 - c2; + + // Take the average instead of the median + z_values[x][y] = (a + b + c) / 3.0; + TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(x, y, z_values[x][y])); + + // Median is robust (ignores outliers). + // z_values[x][y] = (a < b) ? ((b < c) ? b : (c < a) ? a : c) + // : ((c < b) ? b : (a < c) ? a : c); +} + +//Enable this if your SCARA uses 180° of total area +//#define EXTRAPOLATE_FROM_EDGE + +#if ENABLED(EXTRAPOLATE_FROM_EDGE) + #if GRID_MAX_POINTS_X < GRID_MAX_POINTS_Y + #define HALF_IN_X + #elif GRID_MAX_POINTS_Y < GRID_MAX_POINTS_X + #define HALF_IN_Y + #endif +#endif + +/** + * Fill in the unprobed points (corners of circular print surface) + * using linear extrapolation, away from the center. + */ +void extrapolate_unprobed_bed_level() { + #ifdef HALF_IN_X + constexpr uint8_t ctrx2 = 0, xlen = GRID_MAX_POINTS_X - 1; + #else + constexpr uint8_t ctrx1 = (GRID_MAX_POINTS_X - 1) / 2, // left-of-center + ctrx2 = (GRID_MAX_POINTS_X) / 2, // right-of-center + xlen = ctrx1; + #endif + + #ifdef HALF_IN_Y + constexpr uint8_t ctry2 = 0, ylen = GRID_MAX_POINTS_Y - 1; + #else + constexpr uint8_t ctry1 = (GRID_MAX_POINTS_Y - 1) / 2, // top-of-center + ctry2 = (GRID_MAX_POINTS_Y) / 2, // bottom-of-center + ylen = ctry1; + #endif + + LOOP_LE_N(xo, xlen) + LOOP_LE_N(yo, ylen) { + uint8_t x2 = ctrx2 + xo, y2 = ctry2 + yo; + #ifndef HALF_IN_X + const uint8_t x1 = ctrx1 - xo; + #endif + #ifndef HALF_IN_Y + const uint8_t y1 = ctry1 - yo; + #ifndef HALF_IN_X + extrapolate_one_point(x1, y1, +1, +1); // left-below + + + #endif + extrapolate_one_point(x2, y1, -1, +1); // right-below - + + #endif + #ifndef HALF_IN_X + extrapolate_one_point(x1, y2, +1, -1); // left-above + - + #endif + extrapolate_one_point(x2, y2, -1, -1); // right-above - - + } + +} + +void print_bilinear_leveling_grid() { + SERIAL_ECHOLNPGM("Bilinear Leveling Grid:"); + print_2d_array(GRID_MAX_POINTS_X, GRID_MAX_POINTS_Y, 3, + [](const uint8_t ix, const uint8_t iy) { return z_values[ix][iy]; } + ); +} + +#if ENABLED(ABL_BILINEAR_SUBDIVISION) + + #define ABL_GRID_POINTS_VIRT_X (GRID_MAX_POINTS_X - 1) * (BILINEAR_SUBDIVISIONS) + 1 + #define ABL_GRID_POINTS_VIRT_Y (GRID_MAX_POINTS_Y - 1) * (BILINEAR_SUBDIVISIONS) + 1 + #define ABL_TEMP_POINTS_X (GRID_MAX_POINTS_X + 2) + #define ABL_TEMP_POINTS_Y (GRID_MAX_POINTS_Y + 2) + float z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y]; + xy_pos_t bilinear_grid_spacing_virt; + xy_float_t bilinear_grid_factor_virt; + + void print_bilinear_leveling_grid_virt() { + SERIAL_ECHOLNPGM("Subdivided with CATMULL ROM Leveling Grid:"); + print_2d_array(ABL_GRID_POINTS_VIRT_X, ABL_GRID_POINTS_VIRT_Y, 5, + [](const uint8_t ix, const uint8_t iy) { return z_values_virt[ix][iy]; } + ); + } + + #define LINEAR_EXTRAPOLATION(E, I) ((E) * 2 - (I)) + float bed_level_virt_coord(const uint8_t x, const uint8_t y) { + uint8_t ep = 0, ip = 1; + if (x > GRID_MAX_POINTS_X + 1 || y > GRID_MAX_POINTS_Y + 1) { + // The requested point requires extrapolating two points beyond the mesh. + // These values are only requested for the edges of the mesh, which are always an actual mesh point, + // and do not require interpolation. When interpolation is not needed, this "Mesh + 2" point is + // cancelled out in bed_level_virt_cmr and does not impact the result. Return 0.0 rather than + // making this function more complex by extrapolating two points. + return 0.0; + } + if (!x || x == ABL_TEMP_POINTS_X - 1) { + if (x) { + ep = GRID_MAX_POINTS_X - 1; + ip = GRID_MAX_POINTS_X - 2; + } + if (WITHIN(y, 1, ABL_TEMP_POINTS_Y - 2)) + return LINEAR_EXTRAPOLATION( + z_values[ep][y - 1], + z_values[ip][y - 1] + ); + else + return LINEAR_EXTRAPOLATION( + bed_level_virt_coord(ep + 1, y), + bed_level_virt_coord(ip + 1, y) + ); + } + if (!y || y == ABL_TEMP_POINTS_Y - 1) { + if (y) { + ep = GRID_MAX_POINTS_Y - 1; + ip = GRID_MAX_POINTS_Y - 2; + } + if (WITHIN(x, 1, ABL_TEMP_POINTS_X - 2)) + return LINEAR_EXTRAPOLATION( + z_values[x - 1][ep], + z_values[x - 1][ip] + ); + else + return LINEAR_EXTRAPOLATION( + bed_level_virt_coord(x, ep + 1), + bed_level_virt_coord(x, ip + 1) + ); + } + return z_values[x - 1][y - 1]; + } + + static float bed_level_virt_cmr(const float p[4], const uint8_t i, const float t) { + return ( + p[i-1] * -t * sq(1 - t) + + p[i] * (2 - 5 * sq(t) + 3 * t * sq(t)) + + p[i+1] * t * (1 + 4 * t - 3 * sq(t)) + - p[i+2] * sq(t) * (1 - t) + ) * 0.5f; + } + + static float bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const float &tx, const float &ty) { + float row[4], column[4]; + LOOP_L_N(i, 4) { + LOOP_L_N(j, 4) { + column[j] = bed_level_virt_coord(i + x - 1, j + y - 1); + } + row[i] = bed_level_virt_cmr(column, 1, ty); + } + return bed_level_virt_cmr(row, 1, tx); + } + + void bed_level_virt_interpolate() { + bilinear_grid_spacing_virt = bilinear_grid_spacing / (BILINEAR_SUBDIVISIONS); + bilinear_grid_factor_virt = bilinear_grid_spacing_virt.reciprocal(); + LOOP_L_N(y, GRID_MAX_POINTS_Y) + LOOP_L_N(x, GRID_MAX_POINTS_X) + LOOP_L_N(ty, BILINEAR_SUBDIVISIONS) + LOOP_L_N(tx, BILINEAR_SUBDIVISIONS) { + if ((ty && y == (GRID_MAX_POINTS_Y) - 1) || (tx && x == (GRID_MAX_POINTS_X) - 1)) + continue; + z_values_virt[x * (BILINEAR_SUBDIVISIONS) + tx][y * (BILINEAR_SUBDIVISIONS) + ty] = + bed_level_virt_2cmr( + x + 1, + y + 1, + (float)tx / (BILINEAR_SUBDIVISIONS), + (float)ty / (BILINEAR_SUBDIVISIONS) + ); + } + } +#endif // ABL_BILINEAR_SUBDIVISION + +// Refresh after other values have been updated +void refresh_bed_level() { + bilinear_grid_factor = bilinear_grid_spacing.reciprocal(); + TERN_(ABL_BILINEAR_SUBDIVISION, bed_level_virt_interpolate()); +} + +#if ENABLED(ABL_BILINEAR_SUBDIVISION) + #define ABL_BG_SPACING(A) bilinear_grid_spacing_virt.A + #define ABL_BG_FACTOR(A) bilinear_grid_factor_virt.A + #define ABL_BG_POINTS_X ABL_GRID_POINTS_VIRT_X + #define ABL_BG_POINTS_Y ABL_GRID_POINTS_VIRT_Y + #define ABL_BG_GRID(X,Y) z_values_virt[X][Y] +#else + #define ABL_BG_SPACING(A) bilinear_grid_spacing.A + #define ABL_BG_FACTOR(A) bilinear_grid_factor.A + #define ABL_BG_POINTS_X GRID_MAX_POINTS_X + #define ABL_BG_POINTS_Y GRID_MAX_POINTS_Y + #define ABL_BG_GRID(X,Y) z_values[X][Y] +#endif + +// Get the Z adjustment for non-linear bed leveling +float bilinear_z_offset(const xy_pos_t &raw) { + + static float z1, d2, z3, d4, L, D; + + static xy_pos_t prev { -999.999, -999.999 }, ratio; + + // Whole units for the grid line indices. Constrained within bounds. + static xy_int8_t thisg, nextg, lastg { -99, -99 }; + + // XY relative to the probed area + xy_pos_t rel = raw - bilinear_start.asFloat(); + + #if ENABLED(EXTRAPOLATE_BEYOND_GRID) + #define FAR_EDGE_OR_BOX 2 // Keep using the last grid box + #else + #define FAR_EDGE_OR_BOX 1 // Just use the grid far edge + #endif + + if (prev.x != rel.x) { + prev.x = rel.x; + ratio.x = rel.x * ABL_BG_FACTOR(x); + const float gx = constrain(FLOOR(ratio.x), 0, ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX)); + ratio.x -= gx; // Subtract whole to get the ratio within the grid box + + #if DISABLED(EXTRAPOLATE_BEYOND_GRID) + // Beyond the grid maintain height at grid edges + NOLESS(ratio.x, 0); // Never <0 (>1 is ok when nextg.x==thisg.x) + #endif + + thisg.x = gx; + nextg.x = _MIN(thisg.x + 1, ABL_BG_POINTS_X - 1); + } + + if (prev.y != rel.y || lastg.x != thisg.x) { + + if (prev.y != rel.y) { + prev.y = rel.y; + ratio.y = rel.y * ABL_BG_FACTOR(y); + const float gy = constrain(FLOOR(ratio.y), 0, ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX)); + ratio.y -= gy; + + #if DISABLED(EXTRAPOLATE_BEYOND_GRID) + // Beyond the grid maintain height at grid edges + NOLESS(ratio.y, 0); // Never < 0.0. (> 1.0 is ok when nextg.y==thisg.y.) + #endif + + thisg.y = gy; + nextg.y = _MIN(thisg.y + 1, ABL_BG_POINTS_Y - 1); + } + + if (lastg != thisg) { + lastg = thisg; + // Z at the box corners + z1 = ABL_BG_GRID(thisg.x, thisg.y); // left-front + d2 = ABL_BG_GRID(thisg.x, nextg.y) - z1; // left-back (delta) + z3 = ABL_BG_GRID(nextg.x, thisg.y); // right-front + d4 = ABL_BG_GRID(nextg.x, nextg.y) - z3; // right-back (delta) + } + + // Bilinear interpolate. Needed since rel.y or thisg.x has changed. + L = z1 + d2 * ratio.y; // Linear interp. LF -> LB + const float R = z3 + d4 * ratio.y; // Linear interp. RF -> RB + + D = R - L; + } + + const float offset = L + ratio.x * D; // the offset almost always changes + + /* + static float last_offset = 0; + if (ABS(last_offset - offset) > 0.2) { + SERIAL_ECHOLNPAIR("Sudden Shift at x=", rel.x, " / ", bilinear_grid_spacing.x, " -> thisg.x=", thisg.x); + SERIAL_ECHOLNPAIR(" y=", rel.y, " / ", bilinear_grid_spacing.y, " -> thisg.y=", thisg.y); + SERIAL_ECHOLNPAIR(" ratio.x=", ratio.x, " ratio.y=", ratio.y); + SERIAL_ECHOLNPAIR(" z1=", z1, " z2=", z2, " z3=", z3, " z4=", z4); + SERIAL_ECHOLNPAIR(" L=", L, " R=", R, " offset=", offset); + } + last_offset = offset; + //*/ + + return offset; +} + +#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES) + + #define CELL_INDEX(A,V) ((V - bilinear_start.A) * ABL_BG_FACTOR(A)) + + /** + * Prepare a bilinear-leveled linear move on Cartesian, + * splitting the move where it crosses grid borders. + */ + void bilinear_line_to_destination(const feedRate_t &scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) { + // Get current and destination cells for this line + xy_int_t c1 { CELL_INDEX(x, current_position.x), CELL_INDEX(y, current_position.y) }, + c2 { CELL_INDEX(x, destination.x), CELL_INDEX(y, destination.y) }; + LIMIT(c1.x, 0, ABL_BG_POINTS_X - 2); + LIMIT(c1.y, 0, ABL_BG_POINTS_Y - 2); + LIMIT(c2.x, 0, ABL_BG_POINTS_X - 2); + LIMIT(c2.y, 0, ABL_BG_POINTS_Y - 2); + + // Start and end in the same cell? No split needed. + if (c1 == c2) { + current_position = destination; + line_to_current_position(scaled_fr_mm_s); + return; + } + + #define LINE_SEGMENT_END(A) (current_position.A + (destination.A - current_position.A) * normalized_dist) + + float normalized_dist; + xyze_pos_t end; + const xy_int8_t gc { _MAX(c1.x, c2.x), _MAX(c1.y, c2.y) }; + + // Crosses on the X and not already split on this X? + // The x_splits flags are insurance against rounding errors. + if (c2.x != c1.x && TEST(x_splits, gc.x)) { + // Split on the X grid line + CBI(x_splits, gc.x); + end = destination; + destination.x = bilinear_start.x + ABL_BG_SPACING(x) * gc.x; + normalized_dist = (destination.x - current_position.x) / (end.x - current_position.x); + destination.y = LINE_SEGMENT_END(y); + } + // Crosses on the Y and not already split on this Y? + else if (c2.y != c1.y && TEST(y_splits, gc.y)) { + // Split on the Y grid line + CBI(y_splits, gc.y); + end = destination; + destination.y = bilinear_start.y + ABL_BG_SPACING(y) * gc.y; + normalized_dist = (destination.y - current_position.y) / (end.y - current_position.y); + destination.x = LINE_SEGMENT_END(x); + } + else { + // Must already have been split on these border(s) + // This should be a rare case. + current_position = destination; + line_to_current_position(scaled_fr_mm_s); + return; + } + + destination.z = LINE_SEGMENT_END(z); + destination.e = LINE_SEGMENT_END(e); + + // Do the split and look for more borders + bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits); + + // Restore destination from stack + destination = end; + bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits); + } + +#endif // IS_CARTESIAN && !SEGMENT_LEVELED_MOVES + +#endif // AUTO_BED_LEVELING_BILINEAR diff --git a/Marlin/src/feature/bedlevel/abl/abl.h b/Marlin/src/feature/bedlevel/abl/abl.h new file mode 100644 index 0000000..bbe2411 --- /dev/null +++ b/Marlin/src/feature/bedlevel/abl/abl.h @@ -0,0 +1,45 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2020 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <https://www.gnu.org/licenses/>. + * + */ +#pragma once + +#include "../../../inc/MarlinConfigPre.h" + +extern xy_pos_t bilinear_grid_spacing, bilinear_start; +extern xy_float_t bilinear_grid_factor; +extern bed_mesh_t z_values; +float bilinear_z_offset(const xy_pos_t &raw); + +void extrapolate_unprobed_bed_level(); +void print_bilinear_leveling_grid(); +void refresh_bed_level(); +#if ENABLED(ABL_BILINEAR_SUBDIVISION) + void print_bilinear_leveling_grid_virt(); + void bed_level_virt_interpolate(); +#endif + +#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES) + void bilinear_line_to_destination(const feedRate_t &scaled_fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF); +#endif + +#define _GET_MESH_X(I) float(bilinear_start.x + (I) * bilinear_grid_spacing.x) +#define _GET_MESH_Y(J) float(bilinear_start.y + (J) * bilinear_grid_spacing.y) +#define Z_VALUES_ARR z_values |