From 91841d75c9a473e15932bd4589d64ee59efe947e Mon Sep 17 00:00:00 2001 From: oldmcg Date: Thu, 11 May 2017 22:33:47 -0500 Subject: [PATCH] UBL_DELTA (#6695) UBL on Delta's.... Should be close! Should not affect any Cartesian printer. --- Marlin/Conditionals_post.h | 36 +++- Marlin/G26_Mesh_Validation_Tool.cpp | 114 ++++++------- Marlin/Marlin.h | 67 +++++++- Marlin/Marlin_main.cpp | 105 +++++------- Marlin/SanityCheck.h | 57 ++++--- Marlin/planner.cpp | 45 +++++ Marlin/ubl.cpp | 2 +- Marlin/ubl.h | 12 +- Marlin/ubl_G29.cpp | 106 +++++++----- Marlin/ubl_motion.cpp | 256 ++++++++++++++++++++++++++-- 10 files changed, 591 insertions(+), 209 deletions(-) diff --git a/Marlin/Conditionals_post.h b/Marlin/Conditionals_post.h index 3edc8dfe8..2a1f4e0b8 100644 --- a/Marlin/Conditionals_post.h +++ b/Marlin/Conditionals_post.h @@ -730,11 +730,16 @@ /** * Set granular options based on the specific type of leveling */ + + #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(DELTA) + #define UBL_DELTA + #endif + #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 HAS_ABL (ABL_PLANAR || ABL_GRID || ENABLED(AUTO_BED_LEVELING_UBL)) #define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) - #define PLANNER_LEVELING (ABL_PLANAR || ABL_GRID || ENABLED(MESH_BED_LEVELING)) + #define PLANNER_LEVELING (ABL_PLANAR || ABL_GRID || ENABLED(MESH_BED_LEVELING) || ENABLED(UBL_DELTA)) #define HAS_PROBING_PROCEDURE (HAS_ABL || ENABLED(Z_MIN_PROBE_REPEATABILITY_TEST)) #if HAS_PROBING_PROCEDURE #define PROBE_BED_WIDTH abs(RIGHT_PROBE_BED_POSITION - (LEFT_PROBE_BED_POSITION)) @@ -779,12 +784,13 @@ #define MANUAL_PROBE_HEIGHT Z_HOMING_HEIGHT #endif - #if IS_KINEMATIC - // Check for this in the code instead - #define MIN_PROBE_X X_MIN_POS - #define MAX_PROBE_X X_MAX_POS - #define MIN_PROBE_Y Y_MIN_POS - #define MAX_PROBE_Y Y_MAX_POS + #if ENABLED(DELTA) + // These will be further constrained in code, but UBL_PROBE_PT values + // cannot be compile-time verified within the radius. + #define MIN_PROBE_X (-DELTA_PRINTABLE_RADIUS) + #define MAX_PROBE_X ( DELTA_PRINTABLE_RADIUS) + #define MIN_PROBE_Y (-DELTA_PRINTABLE_RADIUS) + #define MAX_PROBE_Y ( DELTA_PRINTABLE_RADIUS) #else // Boundaries for probing based on set limits #define MIN_PROBE_X (max(X_MIN_POS, X_MIN_POS + X_PROBE_OFFSET_FROM_EXTRUDER)) @@ -814,4 +820,20 @@ #define LCD_TIMEOUT_TO_STATUS 15000 #endif + /** + * DELTA_SEGMENT_MIN_LENGTH for UBL_DELTA + */ + + #if ENABLED(UBL_DELTA) + #ifndef DELTA_SEGMENT_MIN_LENGTH + #if IS_SCARA + #define DELTA_SEGMENT_MIN_LENGTH 0.25 // SCARA minimum segment size is 0.25mm + #elif ENABLED(DELTA) + #define DELTA_SEGMENT_MIN_LENGTH 0.10 // mm (still subject to DELTA_SEGMENTS_PER_SECOND) + #else // CARTESIAN + #define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation) + #endif + #endif + #endif + #endif // CONDITIONALS_POST_H diff --git a/Marlin/G26_Mesh_Validation_Tool.cpp b/Marlin/G26_Mesh_Validation_Tool.cpp index 896eb4b8b..6b862358d 100644 --- a/Marlin/G26_Mesh_Validation_Tool.cpp +++ b/Marlin/G26_Mesh_Validation_Tool.cpp @@ -122,7 +122,7 @@ // External references - extern float feedrate; + extern float feedrate_mm_s; // must set before calling prepare_move_to_destination extern Planner planner; #if ENABLED(ULTRA_LCD) extern char lcd_status_message[]; @@ -130,6 +130,7 @@ extern float destination[XYZE]; void set_destination_to_current(); void set_current_to_destination(); + void prepare_move_to_destination(); float code_value_float(); float code_value_linear_units(); float code_value_axis_units(const AxisEnum axis); @@ -137,9 +138,6 @@ bool code_has_value(); void lcd_init(); void lcd_setstatuspgm(const char* const message, const uint8_t level); - bool prepare_move_to_destination_cartesian(); - void line_to_destination(); - void line_to_destination(float); void sync_plan_position_e(); void chirp_at_user(); @@ -182,6 +180,13 @@ static int16_t g26_repeats; + void G26_line_to_destination(const float &feed_rate) { + const float save_feedrate = feedrate_mm_s; + feedrate_mm_s = feed_rate; // use specified feed rate + prepare_move_to_destination(); // will ultimately call ubl_line_to_destination_cartesian or ubl_prepare_linear_move_to for UBL_DELTA + feedrate_mm_s = save_feedrate; // restore global feed rate + } + /** * G26: Mesh Validation Pattern generation. * @@ -271,21 +276,10 @@ const float circle_x = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), circle_y = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); - // Let's do a couple of quick sanity checks. We can pull this code out later if we never see it catch a problem - #ifdef DELTA - if (HYPOT2(circle_x, circle_y) > sq(DELTA_PRINTABLE_RADIUS)) { - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Attempt to print outside of DELTA_PRINTABLE_RADIUS."); - goto LEAVE; - } - #endif + // If this mesh location is outside the printable_radius, skip it. - // TODO: Change this to use `position_is_reachable` - if (!WITHIN(circle_x, X_MIN_POS, X_MAX_POS) || !WITHIN(circle_y, Y_MIN_POS, Y_MAX_POS)) { - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Attempt to print off the bed."); - goto LEAVE; - } + if ( ! position_is_reachable_raw_xy( circle_x, circle_y )) + continue; xi = location.x_index; // Just to shrink the next few lines and make them easier to understand yi = location.y_index; @@ -333,9 +327,11 @@ y = circle_y + sin_table[tmp_div_30], xe = circle_x + cos_table[tmp_div_30 + 1], ye = circle_y + sin_table[tmp_div_30 + 1]; - #ifdef DELTA - if (HYPOT2(x, y) > sq(DELTA_PRINTABLE_RADIUS)) // Check to make sure this part of - continue; // the 'circle' is on the bed. If + #if IS_KINEMATIC + // Check to make sure this segment is entirely on the bed, skip if not. + if (( ! position_is_reachable_raw_xy( x , y )) || + ( ! position_is_reachable_raw_xy( xe, ye ))) + continue; #else // not, we need to skip x = constrain(x, X_MIN_POS + 1, X_MAX_POS - 1); // This keeps us from bumping the endstops y = constrain(y, Y_MIN_POS + 1, Y_MAX_POS - 1); @@ -463,18 +459,22 @@ sy = ey = constrain(pgm_read_float(&ubl.mesh_index_to_ypos[j]), Y_MIN_POS + 1, Y_MAX_POS - 1); ex = constrain(ex, X_MIN_POS + 1, X_MAX_POS - 1); - if (ubl.g26_debug_flag) { - SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx); - SERIAL_ECHOPAIR(", sy=", sy); - SERIAL_ECHOPAIR(") -> (ex=", ex); - SERIAL_ECHOPAIR(", ey=", ey); - SERIAL_CHAR(')'); - SERIAL_EOL; - //debug_current_and_destination(PSTR("Connecting horizontal line.")); - } + if (( position_is_reachable_raw_xy( sx, sy )) && + ( position_is_reachable_raw_xy( ex, ey ))) { - print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); - bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again + if (ubl.g26_debug_flag) { + SERIAL_ECHOPAIR(" Connecting with horizontal line (sx=", sx); + SERIAL_ECHOPAIR(", sy=", sy); + SERIAL_ECHOPAIR(") -> (ex=", ex); + SERIAL_ECHOPAIR(", ey=", ey); + SERIAL_CHAR(')'); + SERIAL_EOL; + //debug_current_and_destination(PSTR("Connecting horizontal line.")); + } + + print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); + } + bit_set(horizontal_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if we skipped it } } @@ -494,17 +494,21 @@ sy = constrain(sy, Y_MIN_POS + 1, Y_MAX_POS - 1); ey = constrain(ey, Y_MIN_POS + 1, Y_MAX_POS - 1); - if (ubl.g26_debug_flag) { - SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx); - SERIAL_ECHOPAIR(", sy=", sy); - SERIAL_ECHOPAIR(") -> (ex=", ex); - SERIAL_ECHOPAIR(", ey=", ey); - SERIAL_CHAR(')'); - SERIAL_EOL; - debug_current_and_destination(PSTR("Connecting vertical line.")); + if (( position_is_reachable_raw_xy( sx, sy )) && + ( position_is_reachable_raw_xy( ex, ey ))) { + + if (ubl.g26_debug_flag) { + SERIAL_ECHOPAIR(" Connecting with vertical line (sx=", sx); + SERIAL_ECHOPAIR(", sy=", sy); + SERIAL_ECHOPAIR(") -> (ex=", ex); + SERIAL_ECHOPAIR(", ey=", ey); + SERIAL_CHAR(')'); + SERIAL_EOL; + debug_current_and_destination(PSTR("Connecting vertical line.")); + } + print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); } - print_line_from_here_to_there(LOGICAL_X_POSITION(sx), LOGICAL_Y_POSITION(sy), layer_height, LOGICAL_X_POSITION(ex), LOGICAL_Y_POSITION(ey), layer_height); - bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again + bit_set(vertical_mesh_line_flags, i, j); // Mark it as done so we don't do it again, even if skipped } } } @@ -532,7 +536,7 @@ destination[Z_AXIS] = z; // We know the last_z==z or we wouldn't be in this block of code. destination[E_AXIS] = current_position[E_AXIS]; - ubl_line_to_destination(feed_value, 0); + G26_line_to_destination(feed_value); stepper.synchronize(); set_destination_to_current(); @@ -552,7 +556,7 @@ //if (ubl.g26_debug_flag) debug_current_and_destination(PSTR(" in move_to() doing last move")); - ubl_line_to_destination(feed_value, 0); + G26_line_to_destination(feed_value); //if (ubl.g26_debug_flag) debug_current_and_destination(PSTR(" in move_to() after last move")); @@ -755,20 +759,16 @@ y_pos = current_position[Y_AXIS]; if (code_seen('X')) { - x_pos = code_value_axis_units(X_AXIS); - if (!WITHIN(x_pos, X_MIN_POS, X_MAX_POS)) { - SERIAL_PROTOCOLLNPGM("?Specified X coordinate not plausible."); - return UBL_ERR; - } + x_pos = code_value_float(); } - else if (code_seen('Y')) { - y_pos = code_value_axis_units(Y_AXIS); - if (!WITHIN(y_pos, Y_MIN_POS, Y_MAX_POS)) { - SERIAL_PROTOCOLLNPGM("?Specified Y coordinate not plausible."); - return UBL_ERR; - } + y_pos = code_value_float(); + } + + if ( ! position_is_reachable_xy( x_pos, y_pos )) { + SERIAL_PROTOCOLLNPGM("?Specified X,Y coordinate out of bounds."); + return UBL_ERR; } /** @@ -864,7 +864,7 @@ Total_Prime += 0.25; if (Total_Prime >= EXTRUDE_MAXLENGTH) return UBL_ERR; #endif - ubl_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0); + G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0); stepper.synchronize(); // Without this synchronize, the purge is more consistent, // but because the planner has a buffer, we won't be able @@ -893,7 +893,7 @@ #endif set_destination_to_current(); destination[E_AXIS] += prime_length; - ubl_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0, 0); + G26_line_to_destination(planner.max_feedrate_mm_s[E_AXIS] / 15.0); stepper.synchronize(); set_destination_to_current(); retract_filament(destination); diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index b10541b06..e74ad8ae0 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -429,4 +429,69 @@ void do_blocking_move_to_xy(const float &x, const float &y, const float &fr_mm_s bool axis_unhomed_error(const bool x, const bool y, const bool z); #endif -#endif // MARLIN_H +/** + * position_is_reachable family of functions + */ + +#if IS_KINEMATIC // (DELTA or SCARA) + + #if ENABLED(DELTA) + #define DELTA_PRINTABLE_RADIUS_SQUARED ((float)DELTA_PRINTABLE_RADIUS * (float)DELTA_PRINTABLE_RADIUS ) + #endif + + #if IS_SCARA + extern const float L1, L2; + #endif + + inline bool position_is_reachable_raw_xy( float raw_x, float raw_y ) { + #if ENABLED(DELTA) + return ( HYPOT2( raw_x, raw_y ) <= DELTA_PRINTABLE_RADIUS_SQUARED ); + #elif IS_SCARA + #if MIDDLE_DEAD_ZONE_R > 0 + const float R2 = HYPOT2(raw_x - SCARA_OFFSET_X, raw_y - SCARA_OFFSET_Y); + return R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) && R2 <= sq(L1 + L2); + #else + return HYPOT2(raw_x - SCARA_OFFSET_X, raw_y - SCARA_OFFSET_Y) <= sq(L1 + L2); + #endif + #else // CARTESIAN + #error + #endif + } + + inline bool position_is_reachable_by_probe_raw_xy( float raw_x, float raw_y ) { + + // both the nozzle and the probe must be able to reach the point + + return ( position_is_reachable_raw_xy( raw_x, raw_y ) && + position_is_reachable_raw_xy( + raw_x - X_PROBE_OFFSET_FROM_EXTRUDER, + raw_y - Y_PROBE_OFFSET_FROM_EXTRUDER )); + } + +#else // CARTESIAN + + inline bool position_is_reachable_raw_xy( float raw_x, float raw_y ) { + // note to reviewer: this +/-0.0001 logic is copied from original postion_is_reachable + return WITHIN(raw_x, X_MIN_POS - 0.0001, X_MAX_POS + 0.0001) + && WITHIN(raw_y, Y_MIN_POS - 0.0001, Y_MAX_POS + 0.0001); + } + + inline bool position_is_reachable_by_probe_raw_xy( float raw_x, float raw_y ) { + // note to reviewer: this logic is copied from UBL_G29.cpp and does not contain the +/-0.0001 above + return WITHIN(raw_x, MIN_PROBE_X, MAX_PROBE_X) + && WITHIN(raw_y, MIN_PROBE_Y, MAX_PROBE_Y); + } + +#endif // CARTESIAN + +inline bool position_is_reachable_by_probe_xy( float target_x, float target_y ) { + return position_is_reachable_by_probe_raw_xy( + RAW_X_POSITION( target_x ), + RAW_Y_POSITION( target_y )); +} + +inline bool position_is_reachable_xy( float target_x, float target_y ) { + return position_is_reachable_raw_xy( RAW_X_POSITION( target_x ), RAW_Y_POSITION( target_y )); +} + +#endif //MARLIN_H diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 7eb417db7..983ae939e 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -401,7 +401,7 @@ float constexpr homing_feedrate_mm_s[] = { #endif MMM_TO_MMS(HOMING_FEEDRATE_Z), 0 }; -static float feedrate_mm_s = MMM_TO_MMS(1500.0), saved_feedrate_mm_s; +float feedrate_mm_s = MMM_TO_MMS(1500.0), saved_feedrate_mm_s; int feedrate_percentage = 100, saved_feedrate_percentage, flow_percentage[EXTRUDERS] = ARRAY_BY_EXTRUDERS1(100); @@ -1677,6 +1677,8 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f #if ENABLED(DELTA) + if ( ! position_is_reachable_xy( x, y )) return; + feedrate_mm_s = fr_mm_s ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S; set_destination_to_current(); // sync destination at the start @@ -1731,6 +1733,8 @@ void do_blocking_move_to(const float &x, const float &y, const float &z, const f #elif IS_SCARA + if ( ! position_is_reachable_xy( x, y )) return; + set_destination_to_current(); // If Z needs to raise, do it before moving XY @@ -2351,6 +2355,8 @@ static void clean_up_after_endstop_or_probe_move() { } #endif + if ( ! position_is_reachable_by_probe_xy( x, y )) return NAN; + const float old_feedrate_mm_s = feedrate_mm_s; #if ENABLED(DELTA) @@ -2419,8 +2425,13 @@ static void clean_up_after_endstop_or_probe_move() { #elif ENABLED(AUTO_BED_LEVELING_UBL) + #if ENABLED(UBL_DELTA) + if (( ubl.state.active ) && ( ! enable )) { // leveling from on to off + planner.unapply_leveling(current_position); + } + #endif + ubl.state.active = enable; - //set_current_from_steppers_for_axis(Z_AXIS); #else @@ -3210,37 +3221,6 @@ void unknown_command_error() { #endif // HOST_KEEPALIVE_FEATURE -bool position_is_reachable(const float target[XYZ] - #if HAS_BED_PROBE - , bool by_probe=false - #endif -) { - float dx = RAW_X_POSITION(target[X_AXIS]), - dy = RAW_Y_POSITION(target[Y_AXIS]); - - #if HAS_BED_PROBE - if (by_probe) { - dx -= X_PROBE_OFFSET_FROM_EXTRUDER; - dy -= Y_PROBE_OFFSET_FROM_EXTRUDER; - } - #endif - - #if IS_SCARA - #if MIDDLE_DEAD_ZONE_R > 0 - const float R2 = HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y); - return R2 >= sq(float(MIDDLE_DEAD_ZONE_R)) && R2 <= sq(L1 + L2); - #else - return HYPOT2(dx - SCARA_OFFSET_X, dy - SCARA_OFFSET_Y) <= sq(L1 + L2); - #endif - #elif ENABLED(DELTA) - return HYPOT2(dx, dy) <= sq((float)(DELTA_PRINTABLE_RADIUS)); - #else - const float dz = RAW_Z_POSITION(target[Z_AXIS]); - return WITHIN(dx, X_MIN_POS - 0.0001, X_MAX_POS + 0.0001) - && WITHIN(dy, Y_MIN_POS - 0.0001, Y_MAX_POS + 0.0001) - && WITHIN(dz, Z_MIN_POS - 0.0001, Z_MAX_POS + 0.0001); - #endif -} /************************************************** ***************** GCode Handlers ***************** @@ -3676,18 +3656,12 @@ inline void gcode_G4() { destination[Y_AXIS] = LOGICAL_Y_POSITION(Z_SAFE_HOMING_Y_POINT); destination[Z_AXIS] = current_position[Z_AXIS]; // Z is already at the right height - if (position_is_reachable( - destination - #if HOMING_Z_WITH_PROBE - , true - #endif - ) - ) { + #if HOMING_Z_WITH_PROBE + destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER; + destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER; + #endif - #if HOMING_Z_WITH_PROBE - destination[X_AXIS] -= X_PROBE_OFFSET_FROM_EXTRUDER; - destination[Y_AXIS] -= Y_PROBE_OFFSET_FROM_EXTRUDER; - #endif + if ( position_is_reachable_xy( destination[X_AXIS], destination[Y_AXIS] )) { #if ENABLED(DEBUG_LEVELING_FEATURE) if (DEBUGGING(LEVELING)) DEBUG_POS("Z_SAFE_HOMING", destination); @@ -4612,8 +4586,7 @@ void home_all_axes() { gcode_G28(); } indexIntoAB[xCount][yCount] = abl_probe_index; #endif - float pos[XYZ] = { xProbe, yProbe, 0 }; - if (position_is_reachable(pos)) break; + if (position_is_reachable_xy( xProbe, yProbe )) break; ++abl_probe_index; } @@ -4724,8 +4697,7 @@ void home_all_axes() { gcode_G28(); } #if IS_KINEMATIC // Avoid probing outside the round or hexagonal area - const float pos[XYZ] = { xProbe, yProbe, 0 }; - if (!position_is_reachable(pos, true)) continue; + if (!position_is_reachable_by_probe_xy( xProbe, yProbe )) continue; #endif measured_z = faux ? 0.001 * random(-100, 101) : probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level); @@ -5028,10 +5000,9 @@ void home_all_axes() { gcode_G28(); } */ inline void gcode_G30() { const float xpos = code_seen('X') ? code_value_linear_units() : current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER, - ypos = code_seen('Y') ? code_value_linear_units() : current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER, - pos[XYZ] = { xpos, ypos, LOGICAL_Z_POSITION(0) }; + ypos = code_seen('Y') ? code_value_linear_units() : current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER; - if (!position_is_reachable(pos, true)) return; + if (!position_is_reachable_by_probe_xy( xpos, ypos )) return; // Disable leveling so the planner won't mess with us #if HAS_LEVELING @@ -6222,22 +6193,19 @@ inline void gcode_M42() { bool stow_probe_after_each = code_seen('E'); float X_probe_location = code_seen('X') ? code_value_linear_units() : X_current + X_PROBE_OFFSET_FROM_EXTRUDER; + float Y_probe_location = code_seen('Y') ? code_value_linear_units() : Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER; + #if DISABLED(DELTA) if (!WITHIN(X_probe_location, LOGICAL_X_POSITION(MIN_PROBE_X), LOGICAL_X_POSITION(MAX_PROBE_X))) { out_of_range_error(PSTR("X")); return; } - #endif - - float Y_probe_location = code_seen('Y') ? code_value_linear_units() : Y_current + Y_PROBE_OFFSET_FROM_EXTRUDER; - #if DISABLED(DELTA) if (!WITHIN(Y_probe_location, LOGICAL_Y_POSITION(MIN_PROBE_Y), LOGICAL_Y_POSITION(MAX_PROBE_Y))) { out_of_range_error(PSTR("Y")); return; } #else - float pos[XYZ] = { X_probe_location, Y_probe_location, 0 }; - if (!position_is_reachable(pos, true)) { + if (!position_is_reachable_by_probe_xy(X_probe_location, Y_probe_location)) { SERIAL_PROTOCOLLNPGM("? (X,Y) location outside of probeable radius."); return; } @@ -6335,7 +6303,7 @@ inline void gcode_M42() { #else // If we have gone out too far, we can do a simple fix and scale the numbers // back in closer to the origin. - while (HYPOT(X_current, Y_current) > DELTA_PROBEABLE_RADIUS) { + while ( ! position_is_reachable_by_probe_xy( X_current, Y_current )) { X_current *= 0.8; Y_current *= 0.8; if (verbose_level > 3) { @@ -11138,7 +11106,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { #endif // AUTO_BED_LEVELING_BILINEAR -#if IS_KINEMATIC +#if IS_KINEMATIC && DISABLED(UBL_DELTA) /** * Prepare a linear move in a DELTA or SCARA setup. @@ -11157,6 +11125,9 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { return false; } + // Fail if attempting move outside printable radius + if ( ! position_is_reachable_xy( ltarget[X_AXIS], ltarget[Y_AXIS] )) return true; + // Get the cartesian distances moved in XYZE float difference[XYZE]; LOOP_XYZE(i) difference[i] = ltarget[i] - current_position[i]; @@ -11245,7 +11216,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { // For SCARA scale the feed rate from mm/s to degrees/s // With segments > 1 length is 1 segment, otherwise total length inverse_kinematics(ltarget); - ADJUST_DELTA(logical); + ADJUST_DELTA(ltarget); const float adiff = abs(delta[A_AXIS] - oldA), bdiff = abs(delta[B_AXIS] - oldB); planner.buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], logical[E_AXIS], max(adiff, bdiff) * feed_factor, active_extruder); @@ -11278,7 +11249,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { else #elif ENABLED(AUTO_BED_LEVELING_UBL) if (ubl.state.active) { - ubl_line_to_destination(MMS_SCALED(feedrate_mm_s), active_extruder); + ubl_line_to_destination_cartesian(MMS_SCALED(feedrate_mm_s), active_extruder); return true; } else @@ -11407,12 +11378,19 @@ void prepare_move_to_destination() { #endif #if IS_KINEMATIC - if (prepare_kinematic_move_to(destination)) return; + #if ENABLED(UBL_DELTA) + if (ubl_prepare_linear_move_to(destination,feedrate_mm_s)) return; + #else + if (prepare_kinematic_move_to(destination)) return; + #endif #else #if ENABLED(DUAL_X_CARRIAGE) if (prepare_move_to_destination_dualx()) return; + #elif ENABLED(UBL_DELTA) // will work for CARTESIAN too (smaller segments follow mesh more closely) + if (ubl_prepare_linear_move_to(destination,feedrate_mm_s)) return; + #else + if (prepare_move_to_destination_cartesian()) return; #endif - if (prepare_move_to_destination_cartesian()) return; #endif set_current_to_destination(); @@ -12427,3 +12405,4 @@ void loop() { endstops.report_state(); idle(); } + diff --git a/Marlin/SanityCheck.h b/Marlin/SanityCheck.h index a18d8d557..006bf6829 100644 --- a/Marlin/SanityCheck.h +++ b/Marlin/SanityCheck.h @@ -248,8 +248,9 @@ #if ENABLED(DELTA) #if DISABLED(USE_XMAX_PLUG) && DISABLED(USE_YMAX_PLUG) && DISABLED(USE_ZMAX_PLUG) #error "You probably want to use Max Endstops for DELTA!" - #elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - #error "DELTA is incompatible with ENABLE_LEVELING_FADE_HEIGHT. Please disable it." + #endif + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(UBL_DELTA) + #error "ENABLE_LEVELING_FADE_HEIGHT for DELTA requires UBL_DELTA and AUTO_BED_LEVELING_UBL." #endif #if ABL_GRID #if (GRID_MAX_POINTS_X & 1) == 0 || (GRID_MAX_POINTS_Y & 1) == 0 @@ -430,11 +431,20 @@ static_assert(1 >= 0 * Unified Bed Leveling */ #if ENABLED(AUTO_BED_LEVELING_UBL) - #if ENABLED(DELTA) - #error "AUTO_BED_LEVELING_UBL does not yet support DELTA printers." - #elif DISABLED(NEWPANEL) + #if IS_KINEMATIC + #if ENABLED(DELTA) + #if DISABLED(UBL_DELTA) + #error "AUTO_BED_LEVELING_UBL requires UBL_DELTA for DELTA printers." + #endif + #else // SCARA + #error "AUTO_BED_LEVELING_UBL not supported for SCARA printers." + #endif + #endif + #if DISABLED(NEWPANEL) #error "AUTO_BED_LEVELING_UBL requires an LCD controller." #endif +#elif ENABLED(UBL_DELTA) + #error "UBL_DELTA requires AUTO_BED_LEVELING_UBL." #endif /** @@ -593,11 +603,9 @@ static_assert(1 >= 0 /** * Delta and SCARA have limited bed leveling options */ - #if DISABLED(AUTO_BED_LEVELING_BILINEAR) - #if ENABLED(DELTA) - #error "Only AUTO_BED_LEVELING_BILINEAR is supported for DELTA bed leveling." - #elif ENABLED(SCARA) - #error "Only AUTO_BED_LEVELING_BILINEAR is supported for SCARA bed leveling." + #if IS_KINEMATIC + #if DISABLED(AUTO_BED_LEVELING_BILINEAR) && DISABLED(UBL_DELTA) + #error "Only AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL with UBL_DELTA support DELTA and SCARA bed leveling." #endif #endif @@ -626,18 +634,23 @@ static_assert(1 >= 0 #error "AUTO_BED_LEVELING_UBL requires EEPROM_SETTINGS. Please update your configuration." #elif !WITHIN(GRID_MAX_POINTS_X, 3, 15) || !WITHIN(GRID_MAX_POINTS_Y, 3, 15) #error "GRID_MAX_POINTS_[XY] must be a whole number between 3 and 15." - #elif !WITHIN(UBL_PROBE_PT_1_X, MIN_PROBE_X, MAX_PROBE_X) - #error "The given UBL_PROBE_PT_1_X can't be reached by the Z probe." - #elif !WITHIN(UBL_PROBE_PT_2_X, MIN_PROBE_X, MAX_PROBE_X) - #error "The given UBL_PROBE_PT_2_X can't be reached by the Z probe." - #elif !WITHIN(UBL_PROBE_PT_3_X, MIN_PROBE_X, MAX_PROBE_X) - #error "The given UBL_PROBE_PT_3_X can't be reached by the Z probe." - #elif !WITHIN(UBL_PROBE_PT_1_Y, MIN_PROBE_Y, MAX_PROBE_Y) - #error "The given UBL_PROBE_PT_1_Y can't be reached by the Z probe." - #elif !WITHIN(UBL_PROBE_PT_2_Y, MIN_PROBE_Y, MAX_PROBE_Y) - #error "The given UBL_PROBE_PT_2_Y can't be reached by the Z probe." - #elif !WITHIN(UBL_PROBE_PT_3_Y, MIN_PROBE_Y, MAX_PROBE_Y) - #error "The given UBL_PROBE_PT_3_Y can't be reached by the Z probe." + #endif + #if IS_CARTESIAN + #if !WITHIN(GRID_MAX_POINTS_X, 3, 15) || !WITHIN(GRID_MAX_POINTS_Y, 3, 15) + #error "GRID_MAX_POINTS_[XY] must be a whole number between 3 and 15." + #elif !WITHIN(UBL_PROBE_PT_1_X, MIN_PROBE_X, MAX_PROBE_X) + #error "The given UBL_PROBE_PT_1_X can't be reached by the Z probe." + #elif !WITHIN(UBL_PROBE_PT_2_X, MIN_PROBE_X, MAX_PROBE_X) + #error "The given UBL_PROBE_PT_2_X can't be reached by the Z probe." + #elif !WITHIN(UBL_PROBE_PT_3_X, MIN_PROBE_X, MAX_PROBE_X) + #error "The given UBL_PROBE_PT_3_X can't be reached by the Z probe." + #elif !WITHIN(UBL_PROBE_PT_1_Y, MIN_PROBE_Y, MAX_PROBE_Y) + #error "The given UBL_PROBE_PT_1_Y can't be reached by the Z probe." + #elif !WITHIN(UBL_PROBE_PT_2_Y, MIN_PROBE_Y, MAX_PROBE_Y) + #error "The given UBL_PROBE_PT_2_Y can't be reached by the Z probe." + #elif !WITHIN(UBL_PROBE_PT_3_Y, MIN_PROBE_Y, MAX_PROBE_Y) + #error "The given UBL_PROBE_PT_3_Y can't be reached by the Z probe." + #endif #endif #else // AUTO_BED_LEVELING_3POINT #if !WITHIN(ABL_PROBE_PT_1_X, MIN_PROBE_X, MAX_PROBE_X) diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index af6808fa2..8c641fc26 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -63,6 +63,7 @@ #include "temperature.h" #include "ultralcd.h" #include "language.h" +#include "ubl.h" #include "Marlin.h" @@ -533,6 +534,17 @@ void Planner::check_axes_activity() { */ void Planner::apply_leveling(float &lx, float &ly, float &lz) { + #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_DELTA) // probably should also be enabled for UBL without UBL_DELTA + if (!ubl.state.active) return; + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + // if z_fade_height enabled (nonzero) and raw_z above it, no leveling required + if ((planner.z_fade_height) && (planner.z_fade_height <= RAW_Z_POSITION(lz))) return; + lz += ubl.state.z_offset + ( ubl.get_z_correction(lx,ly) * ubl.fade_scaling_factor_for_z(lz)); + #else // no fade + lz += ubl.state.z_offset + ubl.get_z_correction(lx,ly); + #endif // FADE + #endif // UBL + #if HAS_ABL if (!abl_enabled) return; #endif @@ -586,6 +598,39 @@ void Planner::check_axes_activity() { void Planner::unapply_leveling(float logical[XYZ]) { + #if ENABLED(AUTO_BED_LEVELING_UBL) && ENABLED(UBL_DELTA) + + if ( ubl.state.active ) { + + float z_leveled = RAW_Z_POSITION(logical[Z_AXIS]); + float z_ublmesh = ubl.get_z_correction(logical[X_AXIS],logical[Y_AXIS]); + float z_unlevel = z_leveled - ubl.state.z_offset - z_ublmesh; + + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + + // for L=leveled, U=unleveled, M=mesh, O=offset, H=fade_height, + // Given L==U+O+M(1-U/H) (faded mesh correction formula for U 0 ? UBL_MESH_MAX_X : UBL_MESH_MIN_X; - y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? UBL_MESH_MAX_Y : UBL_MESH_MIN_Y; + #if IS_KINEMATIC + x_pos = X_HOME_POS; + y_pos = Y_HOME_POS; + #else // cartesian + x_pos = X_PROBE_OFFSET_FROM_EXTRUDER > 0 ? X_MAX_POS : X_MIN_POS; + y_pos = Y_PROBE_OFFSET_FROM_EXTRUDER < 0 ? Y_MAX_POS : Y_MIN_POS; + #endif } if (code_seen('C')) { @@ -457,6 +462,11 @@ } if (code_seen('H') && code_has_value()) height = code_value_float(); + + if ( !position_is_reachable_xy( x_pos, y_pos )) { + SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius."); + return; + } manually_probe_remaining_mesh(x_pos, y_pos, height, card_thickness, code_seen('O') || code_seen('M')); SERIAL_PROTOCOLLNPGM("G29 P2 finished."); @@ -470,17 +480,25 @@ * - Allow 'G29 P3' to choose a 'reasonable' constant. */ if (c_flag) { - while (repetition_cnt--) { - const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false); - if (location.x_index < 0) break; // No more invalid Mesh Points to populate - ubl.z_values[location.x_index][location.y_index] = ubl_constant; - } - break; - } - else - smart_fill_mesh(); // Do a 'Smart' fill using nearby known values - } break; + if ( repetition_cnt >= ( GRID_MAX_POINTS_X * GRID_MAX_POINTS_Y )) { + for ( uint8_t x = 0; x < GRID_MAX_POINTS_X; x++ ) { + for ( uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++ ) { + ubl.z_values[x][y] = ubl_constant; + } + } + } else { + while (repetition_cnt--) { // this only populates reachable mesh points near + const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, x_pos, y_pos, USE_NOZZLE_AS_REFERENCE, NULL, false); + if (location.x_index < 0) break; // No more reachable invalid Mesh Points to populate + ubl.z_values[location.x_index][location.y_index] = ubl_constant; + } + } + } else { + smart_fill_mesh(); // Do a 'Smart' fill using nearby known values + } + break; + } case 4: // @@ -502,6 +520,12 @@ z2 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_2_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_2_Y), false, g29_verbose_level), z3 = probe_pt(LOGICAL_X_POSITION(UBL_PROBE_PT_3_X), LOGICAL_Y_POSITION(UBL_PROBE_PT_3_Y), true, g29_verbose_level); + if ( isnan(z1) || isnan(z2) || isnan(z3)) { // probe_pt will return NAN if unreachable + SERIAL_ERROR_START; + SERIAL_ERRORLNPGM("Attempt to probe off the bed."); + goto LEAVE; + } + // We need to adjust z1, z2, z3 by the Mesh Height at these points. Just because they are non-zero doesn't mean // the Mesh is tilted! (We need to compensate each probe point by what the Mesh says that location's height is) @@ -710,6 +734,8 @@ ubl.save_ubl_active_state_and_disable(); // we don't do bed level correction because we want the raw data when we probe DEPLOY_PROBE(); + uint16_t max_iterations = ( GRID_MAX_POINTS_X * GRID_MAX_POINTS_Y ); + do { if (ubl_lcd_clicked()) { SERIAL_PROTOCOLLNPGM("\nMesh only partially populated.\n"); @@ -723,27 +749,19 @@ } location = find_closest_mesh_point_of_type(INVALID, lx, ly, USE_PROBE_AS_REFERENCE, NULL, do_furthest); - if (location.x_index >= 0 && location.y_index >= 0) { + + if (location.x_index >= 0) { // mesh point found and is reachable by probe const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); - // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) - if (!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y)) { - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Attempt to probe off the bed."); - ubl.has_control_of_lcd_panel = false; - goto LEAVE; - } const float measured_z = probe_pt(LOGICAL_X_POSITION(rawx), LOGICAL_Y_POSITION(rawy), stow_probe, g29_verbose_level); ubl.z_values[location.x_index][location.y_index] = measured_z; } if (do_ubl_mesh_map) ubl.display_map(map_type); - } while (location.x_index >= 0 && location.y_index >= 0); - - LEAVE: + } while ((location.x_index >= 0) && (--max_iterations)); STOW_PROBE(); ubl.restore_ubl_active_state_and_leave(); @@ -939,17 +957,13 @@ const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); - // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) - if (!WITHIN(rawx, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) { - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Attempt to probe off the bed."); - ubl.has_control_of_lcd_panel = false; - goto LEAVE; - } - const float xProbe = LOGICAL_X_POSITION(rawx), yProbe = LOGICAL_Y_POSITION(rawy); + if ( ! position_is_reachable_raw_xy( rawx, rawy )) { // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points) + break; + } + do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES); LCD_MESSAGEPGM("Moving to next"); @@ -1361,13 +1375,17 @@ rawy = pgm_read_float(&ubl.mesh_index_to_ypos[j]); // If using the probe as the reference there are some unreachable locations. + // Also for round beds, there are grid points outside the bed that nozzle can't reach. // Prune them from the list and ignore them till the next Phase (manual nozzle probing). - if (probe_as_reference == USE_PROBE_AS_REFERENCE && - (!WITHIN(rawx, MIN_PROBE_X, MAX_PROBE_X) || !WITHIN(rawy, MIN_PROBE_Y, MAX_PROBE_Y)) - ) continue; + bool reachable = probe_as_reference ? + position_is_reachable_by_probe_raw_xy( rawx, rawy ) : + position_is_reachable_raw_xy( rawx, rawy ); - // Unreachable. Check if it's the closest location to the nozzle. + if ( ! reachable ) + continue; + + // Reachable. Check if it's the closest location to the nozzle. // Add in a weighting factor that considers the current location of the nozzle. const float mx = LOGICAL_X_POSITION(rawx), // Check if we can probe this mesh location @@ -1415,7 +1433,13 @@ uint16_t not_done[16]; int32_t round_off; + if ( ! position_is_reachable_xy( lx, ly )) { + SERIAL_PROTOCOLLNPGM("(X,Y) outside printable radius."); + return; + } + ubl.save_ubl_active_state_and_disable(); + memset(not_done, 0xFF, sizeof(not_done)); LCD_MESSAGEPGM("Fine Tuning Mesh"); @@ -1425,7 +1449,7 @@ do { location = find_closest_mesh_point_of_type(SET_IN_BITMAP, lx, ly, USE_NOZZLE_AS_REFERENCE, not_done, false); - if (location.x_index < 0 && location.y_index < 0) continue; // abort if we can't find any more points. + if (location.x_index < 0 ) break; // stop when we can't find any more reachable points. bit_clear(not_done, location.x_index, location.y_index); // Mark this location as 'adjusted' so we will find a // different location the next time through the loop @@ -1433,12 +1457,8 @@ const float rawx = pgm_read_float(&ubl.mesh_index_to_xpos[location.x_index]), rawy = pgm_read_float(&ubl.mesh_index_to_ypos[location.y_index]); - // TODO: Change to use `position_is_reachable` (for SCARA-compatibility) - if (!WITHIN(rawx, UBL_MESH_MIN_X, UBL_MESH_MAX_X) || !WITHIN(rawy, UBL_MESH_MIN_Y, UBL_MESH_MAX_Y)) { // In theory, we don't need this check. - SERIAL_ERROR_START; - SERIAL_ERRORLNPGM("Attempt to edit off the bed."); // This really can't happen, but do the check for now - ubl.has_control_of_lcd_panel = false; - goto FINE_TUNE_EXIT; + if ( ! position_is_reachable_raw_xy( rawx, rawy )) { // SHOULD NOT OCCUR because find_closest_mesh_point_of_type will only return reachable + break; } float new_z = ubl.z_values[location.x_index][location.y_index]; @@ -1494,7 +1514,7 @@ lcd_implementation_clear(); - } while (location.x_index >= 0 && location.y_index >= 0 && (--repetition_cnt>0)); + } while (( location.x_index >= 0 ) && (--repetition_cnt>0)); FINE_TUNE_EXIT: diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index bdc8de15a..aaca8d00d 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -26,11 +26,13 @@ #include "Marlin.h" #include "ubl.h" #include "planner.h" + #include "stepper.h" #include #include extern float destination[XYZE]; extern void set_current_to_destination(); + extern float delta_segments_per_second; static void debug_echo_axis(const AxisEnum axis) { if (current_position[axis] == destination[axis]) @@ -87,7 +89,7 @@ } - void ubl_line_to_destination(const float &feed_rate, uint8_t extruder) { + void ubl_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 @@ -134,7 +136,7 @@ // 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_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder); + planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder); set_current_to_destination(); if (ubl.g26_debug_flag) @@ -178,7 +180,7 @@ */ if (isnan(z0)) z0 = 0.0; - planner.buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0 + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder); + planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0 + ubl.state.z_offset, end[E_AXIS], feed_rate, extruder); if (ubl.g26_debug_flag) debug_current_and_destination(PSTR("FINAL_MOVE in ubl_line_to_destination()")); @@ -270,7 +272,7 @@ * 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_line() routine will filter it if that happens. + * the planner._buffer_line() routine will filter it if that happens. */ if (y != start[Y_AXIS]) { if (!inf_normalized_flag) { @@ -292,7 +294,7 @@ z_position = end[Z_AXIS]; } - planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); + planner._buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); } //else printf("FIRST MOVE PRUNED "); } @@ -344,7 +346,7 @@ * 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_line() routine will filter it if that happens. + * the planner._buffer_line() routine will filter it if that happens. */ if (x != start[X_AXIS]) { if (!inf_normalized_flag) { @@ -363,7 +365,7 @@ z_position = end[Z_AXIS]; } - planner.buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); + planner._buffer_line(x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); } //else printf("FIRST MOVE PRUNED "); } @@ -426,7 +428,7 @@ e_position = end[E_AXIS]; z_position = end[Z_AXIS]; } - planner.buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); + planner._buffer_line(x, next_mesh_line_y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); current_yi += dyi; yi_cnt--; } @@ -455,7 +457,7 @@ z_position = end[Z_AXIS]; } - planner.buffer_line(next_mesh_line_x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); + planner._buffer_line(next_mesh_line_x, y, z_position + z0 + ubl.state.z_offset, e_position, feed_rate, extruder); current_xi += dxi; xi_cnt--; } @@ -472,4 +474,238 @@ set_current_to_destination(); } -#endif + + #ifdef UBL_DELTA + + #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]; \ + } + + #if IS_SCARA // scale the feed rate from mm/s to degrees/s + static float scara_feed_factor; + static float scara_oldA; + static float scara_oldB; + #endif + + // We don't want additional apply_leveling() performed by regular buffer_line or buffer_line_kinematic, + // so we call _buffer_line directly here. Per-segmented leveling performed first. + + static inline void ubl_buffer_line_segment(const float ltarget[XYZE], const float &fr_mm_s, const uint8_t extruder) { + + #if IS_KINEMATIC + + inverse_kinematics(ltarget); // this writes delta[ABC] from ltarget[XYZ] but does not modify ltarget + float feedrate = fr_mm_s; + + #if IS_SCARA // scale the feed rate from mm/s to degrees/s + float adiff = abs(delta[A_AXIS] - scara_oldA); + float bdiff = abs(delta[B_AXIS] - scara_oldB); + scara_oldA = delta[A_AXIS]; + scara_oldB = delta[B_AXIS]; + feedrate = max(adiff, bdiff) * scara_feed_factor; + #endif + + planner._buffer_line( delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], ltarget[E_AXIS], feedrate, extruder ); + + #else // cartesian + + planner._buffer_line( ltarget[X_AXIS], ltarget[Y_AXIS], ltarget[Z_AXIS], ltarget[E_AXIS], fr_mm_s, extruder ); + + #endif + } + + /** + * Prepare a linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics. + * This calls planner._buffer_line multiple times for small incremental moves. + * Returns true if the caller did NOT update current_position, otherwise false. + */ + + static bool ubl_prepare_linear_move_to(const float ltarget[XYZE], const float &feedrate) { + + if ( ! position_is_reachable_xy( ltarget[X_AXIS], ltarget[Y_AXIS] )) // fail if moving outside reachable boundary + return true; // did not move, so current_position still accurate + + const float difference[XYZE] = { // cartesian distances moved in XYZE + ltarget[X_AXIS] - current_position[X_AXIS], + ltarget[Y_AXIS] - current_position[Y_AXIS], + ltarget[Z_AXIS] - current_position[Z_AXIS], + ltarget[E_AXIS] - current_position[E_AXIS] + }; + + float cartesian_xy_mm = sqrtf( sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) ); // total horizontal xy distance + + #if IS_KINEMATIC + float seconds = cartesian_xy_mm / feedrate; // seconds to move xy distance at requested rate + uint16_t segments = lroundf( delta_segments_per_second * seconds ); // preferred number of segments for distance @ feedrate + uint16_t seglimit = lroundf( cartesian_xy_mm * (1.0/(DELTA_SEGMENT_MIN_LENGTH))); // number of segments at minimum segment length + NOMORE( segments, seglimit ); // limit to minimum segment length (fewer segments) + #else + uint16_t segments = lroundf( cartesian_xy_mm * (1.0/(DELTA_SEGMENT_MIN_LENGTH))); // cartesian fixed segment length + #endif + + NOLESS( segments, 1 ); // must have at least one segment + float inv_segments = 1.0 / segments; // divide once, multiply thereafter + + #if IS_SCARA // scale the feed rate from mm/s to degrees/s + scara_feed_factor = cartesian_xy_mm * inv_segments * feedrate; + scara_oldA = stepper.get_axis_position_degrees(A_AXIS); + scara_oldB = stepper.get_axis_position_degrees(B_AXIS); + #endif + + const float segment_distance[XYZE] = { // length for each segment + difference[X_AXIS] * inv_segments, + difference[Y_AXIS] * inv_segments, + difference[Z_AXIS] * inv_segments, + difference[E_AXIS] * inv_segments + }; + + // Note that E segment distance could vary slightly as z mesh height + // changes for each segment, but small enough to ignore. + + bool above_fade_height = false; + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + if (( planner.z_fade_height != 0 ) && + ( planner.z_fade_height < RAW_Z_POSITION(ltarget[Z_AXIS]) )) { + above_fade_height = true; + } + #endif + + // Only compute leveling per segment if ubl active and target below z_fade_height. + + if (( ! ubl.state.active ) || ( above_fade_height )) { // no mesh leveling + + const float z_offset = ubl.state.active ? ubl.state.z_offset : 0.0; + + float seg_dest[XYZE]; // per-segment destination, + COPY_XYZE( seg_dest, current_position ); // starting from current position + + while (--segments) { + LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; + float ztemp = seg_dest[Z_AXIS]; + seg_dest[Z_AXIS] += z_offset; + ubl_buffer_line_segment( seg_dest, feedrate, active_extruder ); + seg_dest[Z_AXIS] = ztemp; + } + + // Since repeated adding segment_distance accumulates small errors, final move to exact destination. + COPY_XYZE( seg_dest, ltarget ); + seg_dest[Z_AXIS] += z_offset; + ubl_buffer_line_segment( seg_dest, feedrate, active_extruder ); + return false; // moved but did not set_current_to_destination(); + } + + // Otherwise perform per-segment leveling + + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + float fade_scaling_factor = ubl.fade_scaling_factor_for_z(ltarget[Z_AXIS]); + #endif + + float seg_dest[XYZE]; // per-segment destination, initialize to first segment + LOOP_XYZE(i) seg_dest[i] = current_position[i] + segment_distance[i]; + + const float& dx_seg = segment_distance[X_AXIS]; // alias for clarity + const float& dy_seg = segment_distance[Y_AXIS]; + + float rx = RAW_X_POSITION(seg_dest[X_AXIS]); // assume raw vs logical coordinates shifted but not scaled. + float ry = RAW_Y_POSITION(seg_dest[Y_AXIS]); + + do { // for each mesh cell encountered during the move + + // Compute mesh cell invariants that remain constant for all segments within cell. + // Note for cell index, if point is outside the mesh grid (in MESH_INSET perimeter) + // the bilinear interpolation from the adjacent cell within the mesh will still work. + // Inner loop will exit each time (because out of cell bounds) but will come back + // in top of loop and again re-find same adjacent cell and use it, just less efficient + // for mesh inset area. + + int8_t cell_xi = (rx - (UBL_MESH_MIN_X)) * (1.0 / (MESH_X_DIST)); + cell_xi = constrain( cell_xi, 0, (GRID_MAX_POINTS_X) - 1 ); + + int8_t cell_yi = (ry - (UBL_MESH_MIN_Y)) * (1.0 / (MESH_X_DIST)); + cell_yi = constrain( cell_yi, 0, (GRID_MAX_POINTS_Y) - 1 ); + + // float x0 = (UBL_MESH_MIN_X) + ((MESH_X_DIST) * cell_xi ); // lower left cell corner + // float y0 = (UBL_MESH_MIN_Y) + ((MESH_Y_DIST) * cell_yi ); // lower left cell corner + // float x1 = x0 + MESH_X_DIST; // upper right cell corner + // float y1 = y0 + MESH_Y_DIST; // upper right cell corner + + float x0 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi ])); // 64 byte table lookup avoids mul+add + float y0 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi ])); // 64 byte table lookup avoids mul+add + float x1 = pgm_read_float(&(ubl.mesh_index_to_xpos[cell_xi+1])); // 64 byte table lookup avoids mul+add + float y1 = pgm_read_float(&(ubl.mesh_index_to_ypos[cell_yi+1])); // 64 byte table lookup avoids mul+add + + float cx = rx - x0; // cell-relative x + float cy = ry - y0; // cell-relative y + + float z_x0y0 = ubl.z_values[cell_xi ][cell_yi ]; // z at lower left corner + float z_x1y0 = ubl.z_values[cell_xi+1][cell_yi ]; // z at upper left corner + float z_x0y1 = ubl.z_values[cell_xi ][cell_yi+1]; // z at lower right corner + float z_x1y1 = ubl.z_values[cell_xi+1][cell_yi+1]; // z at upper right corner + + if ( isnan( z_x0y0 )) z_x0y0 = 0; // ideally activating ubl.state.active (G29 A) + if ( isnan( z_x1y0 )) z_x1y0 = 0; // should refuse if any invalid mesh points + if ( isnan( z_x0y1 )) z_x0y1 = 0; // in order to avoid isnan tests per cell, + if ( isnan( z_x1y1 )) z_x1y1 = 0; // thus guessing zero for undefined points + + float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0/MESH_X_DIST); // z slope per x along y0 (lower left to lower right) + float z_xmy1 = (z_x1y1 - z_x0y1) * (1.0/MESH_X_DIST); // z slope per x along y1 (upper left to upper right) + + float z_cxy0 = z_x0y0 + z_xmy0 * cx; // z height along y0 at cx + float z_cxy1 = z_x0y1 + z_xmy1 * cx; // z height along y1 at cx + float z_cxyd = z_cxy1 - z_cxy0; // z height difference along cx from y0 to y1 + + float z_cxym = z_cxyd * (1.0/MESH_Y_DIST); // z slope per y along cx from y0 to y1 + float z_cxcy = z_cxy0 + z_cxym * cy; // z height along cx at cy + + // As subsequent segments step through this cell, the z_cxy0 intercept will change + // and the z_cxym slope will change, both as a function of cx within the cell, and + // each change by a constant for fixed segment lengths. + + float z_sxy0 = z_xmy0 * dx_seg; // per-segment adjustment to z_cxy0 + float z_sxym = ( z_xmy1 - z_xmy0 ) * (1.0/MESH_Y_DIST) * dx_seg; // per-segment adjustment to z_cxym + + do { // for all segments within this mesh cell + + z_cxcy += ubl.state.z_offset; + + if ( --segments == 0 ) { // this is last segment, use ltarget for exact + COPY_XYZE( seg_dest, ltarget ); + seg_dest[Z_AXIS] += z_cxcy; + ubl_buffer_line_segment( seg_dest, feedrate, active_extruder ); + return false; // did not set_current_to_destination() + } + + float z_orig = seg_dest[Z_AXIS]; // remember the pre-leveled segment z value + seg_dest[Z_AXIS] = z_orig + z_cxcy; // adjust segment z height per mesh leveling + ubl_buffer_line_segment( seg_dest, feedrate, active_extruder ); + seg_dest[Z_AXIS] = z_orig; // restore pre-leveled z before incrementing + + LOOP_XYZE(i) seg_dest[i] += segment_distance[i]; // adjust seg_dest for next segment + + cx += dx_seg; + cy += dy_seg; + + if ( !WITHIN(cx,0,MESH_X_DIST) || !WITHIN(cy,0,MESH_Y_DIST)) { // done within this cell, break to next + rx = RAW_X_POSITION(seg_dest[X_AXIS]); + ry = RAW_Y_POSITION(seg_dest[Y_AXIS]); + break; + } + + // Next segment still within same mesh cell, adjust the per-segment + // slope and intercept and compute next z height. + + z_cxy0 += z_sxy0; // adjust z_cxy0 by per-segment z_sxy0 + z_cxym += z_sxym; // adjust z_cxym by per-segment z_sxym + z_cxcy = z_cxy0 + z_cxym * cy; // recompute z_cxcy from adjusted slope and intercept + + } while (true); // per-segment loop exits by break after last segment within cell, or by return on final segment + } while (true); // per-cell loop + } // end of function + + #endif // UBL_DELTA + +#endif // AUTO_BED_LEVELING_UBL +