From ef2531558cab8317a772ac766ae5243d742fc89a Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Wed, 29 Nov 2017 15:30:42 -0600 Subject: [PATCH] Add an option to segment leveled moves --- Marlin/Configuration.h | 5 + Marlin/src/feature/bedlevel/abl/abl.cpp | 4 +- Marlin/src/feature/bedlevel/abl/abl.h | 2 +- .../bedlevel/mbl/mesh_bed_leveling.cpp | 114 +++++++++--------- .../feature/bedlevel/mbl/mesh_bed_leveling.h | 5 +- Marlin/src/inc/Conditionals_post.h | 6 +- Marlin/src/inc/SanityCheck.h | 2 + Marlin/src/module/motion.cpp | 107 +++++++++++++--- 8 files changed, 169 insertions(+), 76 deletions(-) diff --git a/Marlin/Configuration.h b/Marlin/Configuration.h index 472fc9dae0..44649e9dd8 100644 --- a/Marlin/Configuration.h +++ b/Marlin/Configuration.h @@ -886,6 +886,11 @@ // The height can be set with M420 Z #define ENABLE_LEVELING_FADE_HEIGHT + // For Cartesian machines, instead of dividing moves on mesh boundaries, + // split up moves into short segments like a Delta. + #define SEGMENT_LEVELED_MOVES + #define LEVELED_SEGMENT_LENGTH 5.0 // (mm) Length of all segments (except the last one) + /** * Enable the G26 Mesh Validation Pattern tool. */ diff --git a/Marlin/src/feature/bedlevel/abl/abl.cpp b/Marlin/src/feature/bedlevel/abl/abl.cpp index aa4082a8ae..59bf4fbeb0 100644 --- a/Marlin/src/feature/bedlevel/abl/abl.cpp +++ b/Marlin/src/feature/bedlevel/abl/abl.cpp @@ -357,7 +357,7 @@ float bilinear_z_offset(const float raw[XYZ]) { return offset; } -#if !IS_KINEMATIC +#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES) #define CELL_INDEX(A,V) ((V - bilinear_start[A##_AXIS]) * ABL_BG_FACTOR(A##_AXIS)) @@ -420,6 +420,6 @@ float bilinear_z_offset(const float raw[XYZ]) { bilinear_line_to_destination(fr_mm_s, x_splits, y_splits); } -#endif // !IS_KINEMATIC +#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 index 316b6c9c6e..0ac1f424bb 100644 --- a/Marlin/src/feature/bedlevel/abl/abl.h +++ b/Marlin/src/feature/bedlevel/abl/abl.h @@ -42,7 +42,7 @@ void bed_level_virt_interpolate(); #endif - #if !IS_KINEMATIC + #if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES) void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF); #endif diff --git a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp index 51b22ea202..7911f6cecd 100644 --- a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp +++ b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp @@ -52,64 +52,68 @@ ZERO(z_values); } - /** - * Prepare a mesh-leveled linear move in a Cartesian setup, - * splitting the move where it crosses mesh borders. - */ - void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits, uint8_t y_splits) { - int cx1 = mbl.cell_index_x(current_position[X_AXIS]), - cy1 = mbl.cell_index_y(current_position[Y_AXIS]), - cx2 = mbl.cell_index_x(destination[X_AXIS]), - cy2 = mbl.cell_index_y(destination[Y_AXIS]); - NOMORE(cx1, GRID_MAX_POINTS_X - 2); - NOMORE(cy1, GRID_MAX_POINTS_Y - 2); - NOMORE(cx2, GRID_MAX_POINTS_X - 2); - NOMORE(cy2, GRID_MAX_POINTS_Y - 2); + #if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES) - if (cx1 == cx2 && cy1 == cy2) { - // Start and end on same mesh square - buffer_line_to_destination(fr_mm_s); - set_current_from_destination(); - return; + /** + * Prepare a mesh-leveled linear move in a Cartesian setup, + * splitting the move where it crosses mesh borders. + */ + void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits, uint8_t y_splits) { + int cx1 = mbl.cell_index_x(current_position[X_AXIS]), + cy1 = mbl.cell_index_y(current_position[Y_AXIS]), + cx2 = mbl.cell_index_x(destination[X_AXIS]), + cy2 = mbl.cell_index_y(destination[Y_AXIS]); + NOMORE(cx1, GRID_MAX_POINTS_X - 2); + NOMORE(cy1, GRID_MAX_POINTS_Y - 2); + NOMORE(cx2, GRID_MAX_POINTS_X - 2); + NOMORE(cy2, GRID_MAX_POINTS_Y - 2); + + if (cx1 == cx2 && cy1 == cy2) { + // Start and end on same mesh square + buffer_line_to_destination(fr_mm_s); + set_current_from_destination(); + return; + } + + #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist) + + float normalized_dist, end[XYZE]; + + // Split at the left/front border of the right/top square + const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); + if (cx2 != cx1 && TEST(x_splits, gcx)) { + COPY(end, destination); + destination[X_AXIS] = mbl.index_to_xpos[gcx]; + normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]); + destination[Y_AXIS] = MBL_SEGMENT_END(Y); + CBI(x_splits, gcx); + } + else if (cy2 != cy1 && TEST(y_splits, gcy)) { + COPY(end, destination); + destination[Y_AXIS] = mbl.index_to_ypos[gcy]; + normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]); + destination[X_AXIS] = MBL_SEGMENT_END(X); + CBI(y_splits, gcy); + } + else { + // Already split on a border + buffer_line_to_destination(fr_mm_s); + set_current_from_destination(); + return; + } + + destination[Z_AXIS] = MBL_SEGMENT_END(Z); + destination[E_AXIS] = MBL_SEGMENT_END(E); + + // Do the split and look for more borders + mesh_line_to_destination(fr_mm_s, x_splits, y_splits); + + // Restore destination from stack + COPY(destination, end); + mesh_line_to_destination(fr_mm_s, x_splits, y_splits); } - #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist) - - float normalized_dist, end[XYZE]; - - // Split at the left/front border of the right/top square - const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); - if (cx2 != cx1 && TEST(x_splits, gcx)) { - COPY(end, destination); - destination[X_AXIS] = mbl.index_to_xpos[gcx]; - normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]); - destination[Y_AXIS] = MBL_SEGMENT_END(Y); - CBI(x_splits, gcx); - } - else if (cy2 != cy1 && TEST(y_splits, gcy)) { - COPY(end, destination); - destination[Y_AXIS] = mbl.index_to_ypos[gcy]; - normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]); - destination[X_AXIS] = MBL_SEGMENT_END(X); - CBI(y_splits, gcy); - } - else { - // Already split on a border - buffer_line_to_destination(fr_mm_s); - set_current_from_destination(); - return; - } - - destination[Z_AXIS] = MBL_SEGMENT_END(Z); - destination[E_AXIS] = MBL_SEGMENT_END(E); - - // Do the split and look for more borders - mesh_line_to_destination(fr_mm_s, x_splits, y_splits); - - // Restore destination from stack - COPY(destination, end); - mesh_line_to_destination(fr_mm_s, x_splits, y_splits); - } + #endif // IS_CARTESIAN && !SEGMENT_LEVELED_MOVES void mbl_mesh_report() { SERIAL_PROTOCOLLNPGM("Num X,Y: " STRINGIFY(GRID_MAX_POINTS_X) "," STRINGIFY(GRID_MAX_POINTS_Y)); diff --git a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h index 034328713c..f183cc7d68 100644 --- a/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h +++ b/Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h @@ -110,8 +110,9 @@ public: extern mesh_bed_leveling mbl; // Support functions, which may be embedded in the class later - -void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF); +#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES) + void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF); +#endif void mbl_mesh_report(); diff --git a/Marlin/src/inc/Conditionals_post.h b/Marlin/src/inc/Conditionals_post.h index 3795cdf999..95a8b6ba9b 100644 --- a/Marlin/src/inc/Conditionals_post.h +++ b/Marlin/src/inc/Conditionals_post.h @@ -934,7 +934,7 @@ /** * Set granular options based on the specific type of leveling */ -#define UBL_DELTA (ENABLED(AUTO_BED_LEVELING_UBL) && (ENABLED(DELTA) || ENABLED(UBL_GRANULAR_SEGMENTATION_FOR_CARTESIAN))) +#define UBL_DELTA (ENABLED(AUTO_BED_LEVELING_UBL) && (ENABLED(DELTA) || ENABLED(SEGMENT_LEVELED_MOVES))) #define ABL_PLANAR (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_3POINT)) #define ABL_GRID (ENABLED(AUTO_BED_LEVELING_LINEAR) || ENABLED(AUTO_BED_LEVELING_BILINEAR)) #define OLDSCHOOL_ABL (ABL_PLANAR || ABL_GRID) @@ -949,6 +949,10 @@ #define PROBE_BED_HEIGHT abs(BACK_PROBE_BED_POSITION - (FRONT_PROBE_BED_POSITION)) #endif +#if ENABLED(SEGMENT_LEVELED_MOVES) && !defined(LEVELED_SEGMENT_LENGTH) + #define LEVELED_SEGMENT_LENGTH 5 +#endif + /** * Bed Probing rectangular bounds * These can be further constrained in code for Delta and SCARA diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index ff05a2c3f2..f2ba984e28 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -223,6 +223,8 @@ #error "ENABLE_MESH_EDIT_GFX_OVERLAY is now MESH_EDIT_GFX_OVERLAY. Please update your configuration." #elif defined(BABYSTEP_ZPROBE_GFX_REVERSE) #error "BABYSTEP_ZPROBE_GFX_REVERSE is now set by OVERLAY_GFX_REVERSE. Please update your configurations." +#elif defined(UBL_GRANULAR_SEGMENTATION_FOR_CARTESIAN) + #error "UBL_GRANULAR_SEGMENTATION_FOR_CARTESIAN is now SEGMENT_LEVELED_MOVES. Please update your configuration." #endif /** diff --git a/Marlin/src/module/motion.cpp b/Marlin/src/module/motion.cpp index c026712c68..a8bd25ef0f 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -522,8 +522,11 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, // Get the top feedrate of the move in the XY plane const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s); + const float xdiff = rtarget[X_AXIS] - current_position[X_AXIS], + ydiff = rtarget[Y_AXIS] - current_position[Y_AXIS]; + // If the move is only in Z/E don't split up the move - if (rtarget[X_AXIS] == current_position[X_AXIS] && rtarget[Y_AXIS] == current_position[Y_AXIS]) { + if (!xdiff && !ydiff) { planner.buffer_line_kinematic(rtarget, _feedrate_mm_s, active_extruder); return false; } @@ -531,19 +534,15 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, // Fail if attempting move outside printable radius if (!position_is_reachable(rtarget[X_AXIS], rtarget[Y_AXIS])) return true; - // Get the cartesian distances moved in XYZE - const float difference[XYZE] = { - rtarget[X_AXIS] - current_position[X_AXIS], - rtarget[Y_AXIS] - current_position[Y_AXIS], - rtarget[Z_AXIS] - current_position[Z_AXIS], - rtarget[E_AXIS] - current_position[E_AXIS] - }; + // Remaining cartesian distances + const float zdiff = rtarget[Z_AXIS] - current_position[Z_AXIS], + ediff = rtarget[E_AXIS] - current_position[E_AXIS]; // Get the linear distance in XYZ - float cartesian_mm = SQRT(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS])); + float cartesian_mm = SQRT(sq(xdiff) + sq(ydiff) + sq(zdiff)); // If the move is very short, check the E move distance - if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = FABS(difference[E_AXIS]); + if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = FABS(ediff); // No E move either? Game over. if (UNEAR_ZERO(cartesian_mm)) return true; @@ -566,10 +565,10 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, // The approximate length of each segment const float inv_segments = 1.0 / float(segments), segment_distance[XYZE] = { - difference[X_AXIS] * inv_segments, - difference[Y_AXIS] * inv_segments, - difference[Z_AXIS] * inv_segments, - difference[E_AXIS] * inv_segments + xdiff * inv_segments, + ydiff * inv_segments, + zdiff * inv_segments, + ediff * inv_segments }; // SERIAL_ECHOPAIR("mm=", cartesian_mm); @@ -644,6 +643,81 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, #else // !IS_KINEMATIC + #if ENABLED(SEGMENT_LEVELED_MOVES) + + /** + * Prepare a segmented move on a CARTESIAN setup. + * + * This calls planner.buffer_line several times, adding + * small incremental moves. This allows the planner to + * apply more detailed bed leveling to the full move. + */ + inline void segmented_line_to_destination(const float &fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) { + + const float xdiff = destination[X_AXIS] - current_position[X_AXIS], + ydiff = destination[Y_AXIS] - current_position[Y_AXIS]; + + // If the move is only in Z/E don't split up the move + if (!xdiff && !ydiff) { + planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder); + return; + } + + // Remaining cartesian distances + const float zdiff = destination[Z_AXIS] - current_position[Z_AXIS], + ediff = destination[E_AXIS] - current_position[E_AXIS]; + + // Get the linear distance in XYZ + // If the move is very short, check the E move distance + // No E move either? Game over. + float cartesian_mm = SQRT(sq(xdiff) + sq(ydiff) + sq(zdiff)); + if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = FABS(ediff); + if (UNEAR_ZERO(cartesian_mm)) return; + + // The length divided by the segment size + // At least one segment is required + uint16_t segments = cartesian_mm / segment_size; + NOLESS(segments, 1); + + // The approximate length of each segment + const float inv_segments = 1.0 / float(segments), + segment_distance[XYZE] = { + xdiff * inv_segments, + ydiff * inv_segments, + zdiff * inv_segments, + ediff * inv_segments + }; + + // SERIAL_ECHOPAIR("mm=", cartesian_mm); + // SERIAL_ECHOLNPAIR(" segments=", segments); + + // Drop one segment so the last move is to the exact target. + // If there's only 1 segment, loops will be skipped entirely. + --segments; + + // Get the raw current position as starting point + float raw[XYZE]; + COPY(raw, current_position); + + // Calculate and execute the segments + for (uint16_t s = segments + 1; --s;) { + static millis_t next_idle_ms = millis() + 200UL; + thermalManager.manage_heater(); // This returns immediately if not really needed. + if (ELAPSED(millis(), next_idle_ms)) { + next_idle_ms = millis() + 200UL; + idle(); + } + LOOP_XYZE(i) raw[i] += segment_distance[i]; + planner.buffer_line_kinematic(raw, fr_mm_s, active_extruder); + } + + // Since segment_distance is only approximate, + // the final move must be to the exact destination. + planner.buffer_line_kinematic(destination, fr_mm_s, active_extruder); + } + + #endif // SEGMENT_LEVELED_MOVES + /** * Prepare a linear move in a Cartesian setup. * @@ -654,10 +728,13 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, */ inline bool prepare_move_to_destination_cartesian() { #if HAS_MESH - if (planner.leveling_active) { + if (planner.leveling_active && planner.leveling_active_at_z(destination[Z_AXIS])) { #if ENABLED(AUTO_BED_LEVELING_UBL) ubl.line_to_destination_cartesian(MMS_SCALED(feedrate_mm_s), active_extruder); // UBL's motion routine needs to know about return true; // all moves, including Z-only moves. + #elif ENABLED(SEGMENT_LEVELED_MOVES) + segmented_line_to_destination(MMS_SCALED(feedrate_mm_s)); + return false; #else /** * For MBL and ABL-BILINEAR only segment moves when X or Y are involved.