diff --git a/.travis.yml b/.travis.yml index ccdb5cfd9..1a566b474 100644 --- a/.travis.yml +++ b/.travis.yml @@ -79,13 +79,14 @@ script: - opt_set TEMP_SENSOR_3 20 - opt_set TEMP_SENSOR_4 999 - opt_set TEMP_SENSOR_BED 1 - - opt_enable AUTO_BED_LEVELING_UBL DEBUG_LEVELING_FEATURE G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT EEPROM_SETTINGS EEPROM_CHITCHAT G3D_PANEL + - opt_enable AUTO_BED_LEVELING_UBL DEBUG_LEVELING_FEATURE G26_MESH_EDITING ENABLE_LEVELING_FADE_HEIGHT EEPROM_SETTINGS EEPROM_CHITCHAT G3D_PANEL SKEW_CORRECTION - opt_enable_adv CUSTOM_USER_MENUS I2C_POSITION_ENCODERS BABYSTEPPING LIN_ADVANCE NANODLP_Z_SYNC - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} # - # And with a Sled Z Probe + # Add a Sled Z Probe, do non-segmented moves # - opt_enable Z_PROBE_SLED + - opt_disable SEGMENT_LEVELED_MOVES - opt_enable_adv BABYSTEP_ZPROBE_OFFSET DOUBLECLICK_FOR_Z_BABYSTEPPING - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} # @@ -121,7 +122,7 @@ script: - opt_enable ULTIMAKERCONTROLLER SDSUPPORT - opt_enable PRINTCOUNTER NOZZLE_PARK_FEATURE NOZZLE_CLEAN_FEATURE PCA9632 USE_XMAX_PLUG - opt_enable_adv BEZIER_CURVE_SUPPORT EXPERIMENTAL_I2CBUS - - opt_enable_adv ADVANCED_PAUSE_FEATURE PARK_HEAD_ON_PAUSE LCD_INFO_MENU + - opt_enable_adv ADVANCED_PAUSE_FEATURE PARK_HEAD_ON_PAUSE LCD_INFO_MENU M114_DETAIL - opt_set_adv PWM_MOTOR_CURRENT {1300,1300,1250} - opt_set_adv I2C_SLAVE_ADDRESS 63 - build_marlin_pio ${TRAVIS_BUILD_DIR} ${TEST_PLATFORM} diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.cpp b/Marlin/src/feature/bedlevel/ubl/ubl.cpp index 3e6e983ad..89739822f 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl.cpp @@ -55,6 +55,59 @@ safe_delay(10); } + #if ENABLED(UBL_DEVEL_DEBUGGING) + + static void debug_echo_axis(const AxisEnum axis) { + if (current_position[axis] == destination[axis]) + SERIAL_ECHOPGM("-------------"); + else + SERIAL_ECHO_F(destination[X_AXIS], 6); + } + + void debug_current_and_destination(const char *title) { + + // if the title message starts with a '!' it is so important, we are going to + // ignore the status of the g26_debug_flag + if (*title != '!' && !g26_debug_flag) return; + + const float de = destination[E_AXIS] - current_position[E_AXIS]; + + if (de == 0.0) return; // Printing moves only + + const float dx = destination[X_AXIS] - current_position[X_AXIS], + dy = destination[Y_AXIS] - current_position[Y_AXIS], + xy_dist = HYPOT(dx, dy); + + if (xy_dist == 0.0) return; + + SERIAL_ECHOPGM(" fpmm="); + const float fpmm = de / xy_dist; + SERIAL_ECHO_F(fpmm, 6); + + SERIAL_ECHOPGM(" current=( "); + SERIAL_ECHO_F(current_position[X_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_ECHO_F(current_position[Y_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_ECHO_F(current_position[Z_AXIS], 6); + SERIAL_ECHOPGM(", "); + SERIAL_ECHO_F(current_position[E_AXIS], 6); + SERIAL_ECHOPGM(" ) destination=( "); + debug_echo_axis(X_AXIS); + SERIAL_ECHOPGM(", "); + debug_echo_axis(Y_AXIS); + SERIAL_ECHOPGM(", "); + debug_echo_axis(Z_AXIS); + SERIAL_ECHOPGM(", "); + debug_echo_axis(E_AXIS); + SERIAL_ECHOPGM(" ) "); + SERIAL_ECHO(title); + SERIAL_EOL(); + + } + + #endif // UBL_DEVEL_DEBUGGING + int8_t unified_bed_leveling::storage_slot; float unified_bed_leveling::z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y]; @@ -178,7 +231,7 @@ uint8_t error_flag = 0; if (settings.calc_num_meshes() < 1) { - SERIAL_PROTOCOLLNPGM("?Insufficient EEPROM storage for a mesh of this size."); + SERIAL_PROTOCOLLNPGM("?Mesh too big for EEPROM."); error_flag++; } diff --git a/Marlin/src/feature/bedlevel/ubl/ubl.h b/Marlin/src/feature/bedlevel/ubl/ubl.h index d371c1a64..01e93769d 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl.h +++ b/Marlin/src/feature/bedlevel/ubl/ubl.h @@ -23,6 +23,8 @@ #ifndef UNIFIED_BED_LEVELING_H #define UNIFIED_BED_LEVELING_H +//#define UBL_DEVEL_DEBUGGING + #include "../bedlevel.h" #include "../../../module/planner.h" #include "../../../module/motion.h" @@ -37,7 +39,11 @@ // ubl_motion.cpp -void debug_current_and_destination(const char * const title); +#if ENABLED(UBL_DEVEL_DEBUGGING) + void debug_current_and_destination(const char * const title); +#else + FORCE_INLINE void debug_current_and_destination(const char * const title) { UNUSED(title); } +#endif // ubl_G29.cpp @@ -217,9 +223,9 @@ class unified_bed_leveling { const float xratio = (rx0 - mesh_index_to_xpos(x1_i)) * (1.0 / (MESH_X_DIST)), z1 = z_values[x1_i][yi]; - return z1 + xratio * (z_values[min(x1_i, GRID_MAX_POINTS_X - 2) + 1][yi] - z1); // Don't allow x1_i+1 to be past the end of the array - // If it is, it is clamped to the last element of the - // z_values[][] array and no correction is applied. + return z1 + xratio * (z_values[min(x1_i, GRID_MAX_POINTS_X - 2) + 1][yi] - z1); // Don't allow x1_i+1 to be past the end of the array + // If it is, it is clamped to the last element of the + // z_values[][] array and no correction is applied. } // @@ -243,9 +249,9 @@ class unified_bed_leveling { const float yratio = (ry0 - mesh_index_to_ypos(y1_i)) * (1.0 / (MESH_Y_DIST)), z1 = z_values[xi][y1_i]; - return z1 + yratio * (z_values[xi][min(y1_i, GRID_MAX_POINTS_Y - 2) + 1] - z1); // Don't allow y1_i+1 to be past the end of the array - // If it is, it is clamped to the last element of the - // z_values[][] array and no correction is applied. + return z1 + yratio * (z_values[xi][min(y1_i, GRID_MAX_POINTS_Y - 2) + 1] - z1); // Don't allow y1_i+1 to be past the end of the array + // If it is, it is clamped to the last element of the + // z_values[][] array and no correction is applied. } /** @@ -315,8 +321,11 @@ 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, uint8_t e); + #if UBL_SEGMENTED + static bool prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate); + #else + static void line_to_destination_cartesian(const float &fr, const uint8_t e); + #endif #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 d8fbe284a..4bc106976 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -23,93 +23,42 @@ #if ENABLED(AUTO_BED_LEVELING_UBL) - #include "../bedlevel.h" - #include "../../../module/planner.h" - #include "../../../module/stepper.h" - #include "../../../module/motion.h" +#include "../bedlevel.h" +#include "../../../module/planner.h" +#include "../../../module/stepper.h" +#include "../../../module/motion.h" - #if ENABLED(DELTA) - #include "../../../module/delta.h" - #endif +#if ENABLED(DELTA) + #include "../../../module/delta.h" +#endif - #include "../../../Marlin.h" - #include +#include "../../../Marlin.h" +#include - extern float destination[XYZE]; +#if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this + inline void set_current_from_destination() { COPY(current_position, destination); } +#else + extern void set_current_from_destination(); +#endif - #if AVR_AT90USB1286_FAMILY // Teensyduino & Printrboard IDE extensions have compile errors without this - inline void set_current_from_destination() { COPY(current_position, destination); } - #else - extern void set_current_from_destination(); - #endif +#if !UBL_SEGMENTED - static void debug_echo_axis(const AxisEnum axis) { - if (current_position[axis] == destination[axis]) - SERIAL_ECHOPGM("-------------"); - else - SERIAL_ECHO_F(destination[X_AXIS], 6); - } - - void debug_current_and_destination(const char *title) { - - // if the title message starts with a '!' it is so important, we are going to - // ignore the status of the g26_debug_flag - if (*title != '!' && !g26_debug_flag) return; - - const float de = destination[E_AXIS] - current_position[E_AXIS]; - - if (de == 0.0) return; // Printing moves only - - const float dx = destination[X_AXIS] - current_position[X_AXIS], - dy = destination[Y_AXIS] - current_position[Y_AXIS], - xy_dist = HYPOT(dx, dy); - - if (xy_dist == 0.0) return; - - SERIAL_ECHOPGM(" fpmm="); - const float fpmm = de / xy_dist; - SERIAL_ECHO_F(fpmm, 6); - - SERIAL_ECHOPGM(" current=( "); - SERIAL_ECHO_F(current_position[X_AXIS], 6); - SERIAL_ECHOPGM(", "); - SERIAL_ECHO_F(current_position[Y_AXIS], 6); - SERIAL_ECHOPGM(", "); - SERIAL_ECHO_F(current_position[Z_AXIS], 6); - SERIAL_ECHOPGM(", "); - SERIAL_ECHO_F(current_position[E_AXIS], 6); - SERIAL_ECHOPGM(" ) destination=( "); - debug_echo_axis(X_AXIS); - SERIAL_ECHOPGM(", "); - debug_echo_axis(Y_AXIS); - SERIAL_ECHOPGM(", "); - debug_echo_axis(Z_AXIS); - SERIAL_ECHOPGM(", "); - debug_echo_axis(E_AXIS); - SERIAL_ECHOPGM(" ) "); - SERIAL_ECHO(title); - SERIAL_EOL(); - - } - - void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, uint8_t extruder) { + void unified_bed_leveling::line_to_destination_cartesian(const float &feed_rate, const 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 * 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]), @@ -117,13 +66,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, @@ -139,11 +88,11 @@ // 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], end[E_AXIS], feed_rate, extruder); + planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS], end[E_AXIS], feed_rate, extruder); 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; } @@ -183,10 +132,10 @@ */ if (isnan(z0)) z0 = 0.0; - planner._buffer_line(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z0, end[E_AXIS], feed_rate, extruder); + 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; @@ -274,7 +223,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_segment() routine will filter it if that happens. */ if (ry != start[Y_AXIS]) { if (!inf_normalized_flag) { @@ -287,12 +236,12 @@ z_position = end[Z_AXIS]; } - planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder); + planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); } //else printf("FIRST MOVE PRUNED "); } 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. @@ -338,7 +287,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_segment() routine will filter it if that happens. */ if (rx != start[X_AXIS]) { if (!inf_normalized_flag) { @@ -351,12 +300,12 @@ z_position = end[Z_AXIS]; } - planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder); + planner.buffer_segment(rx, ry, z_position + z0, e_position, feed_rate, extruder); } //else printf("FIRST MOVE PRUNED "); } 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; @@ -413,7 +362,7 @@ e_position = end[E_AXIS]; z_position = end[Z_AXIS]; } - planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); + planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); current_yi += dyi; yi_cnt--; } @@ -441,7 +390,7 @@ z_position = end[Z_AXIS]; } - planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder); + planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder); current_xi += dxi; xi_cnt--; } @@ -450,7 +399,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; @@ -458,223 +407,222 @@ set_current_from_destination(); } - #if UBL_DELTA +#else // UBL_SEGMENTED - // macro to inline copy exactly 4 floats, don't rely on sizeof operator - #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, scara_oldA, scara_oldB; + #endif + + // 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 (&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], in_raw[E_AXIS], fr, active_extruder); + + #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) + + inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE] + // should move the feedrate scaling to scara inverse_kinematics + + const float adiff = FABS(delta[A_AXIS] - scara_oldA), + bdiff = FABS(delta[B_AXIS] - scara_oldB); + scara_oldA = delta[A_AXIS]; + 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], in_raw[E_AXIS], s_feedrate, active_extruder); + + #else // CARTESIAN + + planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder); + + #endif + } + + #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 + #ifdef LEVELED_SEGMENT_LENGTH + #define DELTA_SEGMENT_MIN_LENGTH LEVELED_SEGMENT_LENGTH + #else + #define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation) + #endif + #endif + + /** + * Prepare a segmented linear move for DELTA/SCARA/CARTESIAN with UBL and FADE semantics. + * This calls planner.buffer_segment multiple times for small incremental moves. + * 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) { + + 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 + + const float total[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] + }; + + const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance + + #if IS_KINEMATIC + const 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 + 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 + const float inv_segments = 1.0 / segments; // divide once, multiply thereafter #if IS_SCARA // scale the feed rate from mm/s to degrees/s - static float scara_feed_factor, scara_oldA, scara_oldB; + 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 - // 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 and kinematics performed first. + const float diff[XYZE] = { + total[X_AXIS] * inv_segments, + total[Y_AXIS] * inv_segments, + total[Z_AXIS] * inv_segments, + total[E_AXIS] * inv_segments + }; - inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) { + // Note that E segment distance could vary slightly as z mesh height + // changes for each segment, but small enough to ignore. - #if ENABLED(DELTA) // apply delta inverse_kinematics + float raw[XYZE] = { + current_position[X_AXIS], + current_position[Y_AXIS], + current_position[Z_AXIS], + current_position[E_AXIS] + }; - DELTA_RAW_IK(); - planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder); - - #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) - - inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE] - // should move the feedrate scaling to scara inverse_kinematics - - const float adiff = FABS(delta[A_AXIS] - scara_oldA), - bdiff = FABS(delta[B_AXIS] - scara_oldB); - scara_oldA = delta[A_AXIS]; - scara_oldB = delta[B_AXIS]; - float s_feedrate = max(adiff, bdiff) * scara_feed_factor; - - planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder); - - #else // CARTESIAN - - planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder); - - #endif - } - - #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 - #ifdef LEVELED_SEGMENT_LENGTH - #define DELTA_SEGMENT_MIN_LENGTH LEVELED_SEGMENT_LENGTH - #else - #define DELTA_SEGMENT_MIN_LENGTH 1.00 // mm (similar to G2/G3 arc segmentation) - #endif - #endif - - /** - * Prepare a segmented 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 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) { - - 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 - - const float total[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] - }; - - const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance - - #if IS_KINEMATIC - const 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 - 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 - const 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 diff[XYZE] = { - total[X_AXIS] * inv_segments, - total[Y_AXIS] * inv_segments, - total[Z_AXIS] * inv_segments, - total[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. - - float raw[XYZE] = { - current_position[X_AXIS], - current_position[Y_AXIS], - current_position[Z_AXIS], - current_position[E_AXIS] - }; - - // 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) { - LOOP_XYZE(i) raw[i] += diff[i]; - ubl_buffer_segment_raw(raw, feedrate); - } - ubl_buffer_segment_raw(rtarget, feedrate); - return false; // moved but did not set_current_from_destination(); + // 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) { + LOOP_XYZE(i) raw[i] += diff[i]; + ubl_buffer_segment_raw(raw, feedrate); } - - // Otherwise perform per-segment leveling - - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]); - #endif - - // increment to first segment destination - LOOP_XYZE(i) raw[i] += diff[i]; - - for(;;) { // 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 = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)), - cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST)); - - cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1); - cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1); - - const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add - y0 = mesh_index_to_ypos(cell_yi); - - float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner - z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner - z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner - z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner - - if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating planner.leveling_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 cx = raw[X_AXIS] - x0, // cell-relative x and y - cy = raw[Y_AXIS] - y0; - - const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right) - 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 (changes for each cx in cell) - - const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx - 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 (changes for each cx in cell) - - // float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop) - - // 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. - - const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0 - z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym - - for(;;) { // for all segments within this mesh cell - - if (--segments == 0) // if this is last segment, use rtarget for exact - COPY(raw, rtarget); - - const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - * fade_scaling_factor // apply fade factor to interpolated mesh height - #endif - ; - - const float z = raw[Z_AXIS]; - raw[Z_AXIS] += z_cxcy; - ubl_buffer_segment_raw(raw, feedrate); - raw[Z_AXIS] = z; - - if (segments == 0) // done with last segment - return false; // did not set_current_from_destination() - - LOOP_XYZE(i) raw[i] += diff[i]; - - cx += diff[X_AXIS]; - cy += diff[Y_AXIS]; - - if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next - break; - - // Next segment still within same mesh cell, adjust the per-segment - // slope and intercept to 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 - - } // segment loop - } // cell loop + ubl_buffer_segment_raw(rtarget, feedrate); + return false; // moved but did not set_current_from_destination(); } - #endif // UBL_DELTA + // Otherwise perform per-segment leveling + + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]); + #endif + + // increment to first segment destination + LOOP_XYZE(i) raw[i] += diff[i]; + + for(;;) { // 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 = (raw[X_AXIS] - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)), + cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * (1.0 / (MESH_X_DIST)); + + cell_xi = constrain(cell_xi, 0, (GRID_MAX_POINTS_X) - 1); + cell_yi = constrain(cell_yi, 0, (GRID_MAX_POINTS_Y) - 1); + + const float x0 = mesh_index_to_xpos(cell_xi), // 64 byte table lookup avoids mul+add + y0 = mesh_index_to_ypos(cell_yi); + + float z_x0y0 = z_values[cell_xi ][cell_yi ], // z at lower left corner + z_x1y0 = z_values[cell_xi+1][cell_yi ], // z at upper left corner + z_x0y1 = z_values[cell_xi ][cell_yi+1], // z at lower right corner + z_x1y1 = z_values[cell_xi+1][cell_yi+1]; // z at upper right corner + + if (isnan(z_x0y0)) z_x0y0 = 0; // ideally activating planner.leveling_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 cx = raw[X_AXIS] - x0, // cell-relative x and y + cy = raw[Y_AXIS] - y0; + + const float z_xmy0 = (z_x1y0 - z_x0y0) * (1.0 / (MESH_X_DIST)), // z slope per x along y0 (lower left to lower right) + 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 (changes for each cx in cell) + + const float z_cxy1 = z_x0y1 + z_xmy1 * cx, // z height along y1 at cx + 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 (changes for each cx in cell) + + // float z_cxcy = z_cxy0 + z_cxym * cy; // interpolated mesh z height along cx at cy (do inside the segment loop) + + // 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. + + const float z_sxy0 = z_xmy0 * diff[X_AXIS], // per-segment adjustment to z_cxy0 + z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * diff[X_AXIS]; // per-segment adjustment to z_cxym + + for(;;) { // for all segments within this mesh cell + + if (--segments == 0) // if this is last segment, use rtarget for exact + COPY(raw, rtarget); + + const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy + #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) + * fade_scaling_factor // apply fade factor to interpolated mesh height + #endif + ; + + const float z = raw[Z_AXIS]; + raw[Z_AXIS] += z_cxcy; + ubl_buffer_segment_raw(raw, feedrate); + raw[Z_AXIS] = z; + + if (segments == 0) // done with last segment + return false; // did not set_current_from_destination() + + LOOP_XYZE(i) raw[i] += diff[i]; + + cx += diff[X_AXIS]; + cy += diff[Y_AXIS]; + + if (!WITHIN(cx, 0, MESH_X_DIST) || !WITHIN(cy, 0, MESH_Y_DIST)) // done within this cell, break to next + break; + + // Next segment still within same mesh cell, adjust the per-segment + // slope and intercept to 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 + + } // segment loop + } // cell loop + } + +#endif // UBL_SEGMENTED #endif // AUTO_BED_LEVELING_UBL diff --git a/Marlin/src/gcode/bedlevel/G26.cpp b/Marlin/src/gcode/bedlevel/G26.cpp index 207fe8c5e..053e879ff 100644 --- a/Marlin/src/gcode/bedlevel/G26.cpp +++ b/Marlin/src/gcode/bedlevel/G26.cpp @@ -220,7 +220,7 @@ mesh_index_pair find_closest_circle_to_print(const float &X, const float &Y) { 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 + prepare_move_to_destination(); // will ultimately call ubl.line_to_destination_cartesian or ubl.prepare_linear_move_to for UBL_SEGMENTED feedrate_mm_s = save_feedrate; // restore global feed rate } @@ -261,16 +261,16 @@ void move_to(const float &rx, const float &ry, const float &z, const float &e_de set_destination_from_current(); } -FORCE_INLINE void move_to(const float where[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); } +FORCE_INLINE void move_to(const float (&where)[XYZE], const float &de) { move_to(where[X_AXIS], where[Y_AXIS], where[Z_AXIS], de); } -void retract_filament(const float where[XYZE]) { +void retract_filament(const float (&where)[XYZE]) { if (!g26_retracted) { // Only retract if we are not already retracted! g26_retracted = true; move_to(where, -1.0 * g26_retraction_multiplier); } } -void recover_filament(const float where[XYZE]) { +void recover_filament(const float (&where)[XYZE]) { if (g26_retracted) { // Only un-retract if we are retracted. move_to(where, 1.2 * g26_retraction_multiplier); g26_retracted = false; diff --git a/Marlin/src/gcode/host/M114.cpp b/Marlin/src/gcode/host/M114.cpp index f5752a1d0..2f176a526 100644 --- a/Marlin/src/gcode/host/M114.cpp +++ b/Marlin/src/gcode/host/M114.cpp @@ -28,7 +28,7 @@ #if ENABLED(M114_DETAIL) - void report_xyze(const float pos[XYZE], const uint8_t n = 4, const uint8_t precision = 3) { + void report_xyze(const float pos[], const uint8_t n = 4, const uint8_t precision = 3) { char str[12]; for (uint8_t i = 0; i < n; i++) { SERIAL_CHAR(' '); @@ -39,7 +39,7 @@ SERIAL_EOL(); } - inline void report_xyz(const float pos[XYZ]) { report_xyze(pos, 3); } + inline void report_xyz(const float pos[]) { report_xyze(pos, 3); } void report_current_position_detail() { @@ -80,8 +80,13 @@ #endif SERIAL_PROTOCOLPGM("Stepper:"); - const float step_count[XYZE] = { stepper.position(X_AXIS), stepper.position(Y_AXIS), stepper.position(Z_AXIS), stepper.position(E_AXIS) }; - report_xyze(step_count, 4, 0); + LOOP_XYZE(i) { + SERIAL_CHAR(' '); + SERIAL_CHAR(axis_codes[i]); + SERIAL_CHAR(':'); + SERIAL_PROTOCOL(stepper.position((AxisEnum)i)); + } + SERIAL_EOL(); #if IS_SCARA const float deg[XYZ] = { diff --git a/Marlin/src/gcode/motion/G2_G3.cpp b/Marlin/src/gcode/motion/G2_G3.cpp index a805c710f..c369be034 100644 --- a/Marlin/src/gcode/motion/G2_G3.cpp +++ b/Marlin/src/gcode/motion/G2_G3.cpp @@ -44,9 +44,9 @@ * options for G2/G3 arc generation. In future these options may be GCode tunable. */ void plan_arc( - float rtarget[XYZE], // Destination position - float *offset, // Center of rotation relative to current_position - uint8_t clockwise // Clockwise? + const float (&cart)[XYZE], // Destination position + const float (&offset)[2], // Center of rotation relative to current_position + const uint8_t clockwise // Clockwise? ) { #if ENABLED(CNC_WORKSPACE_PLANES) AxisEnum p_axis, q_axis, l_axis; @@ -66,10 +66,10 @@ void plan_arc( const float radius = HYPOT(r_P, r_Q), center_P = current_position[p_axis] - r_P, center_Q = current_position[q_axis] - r_Q, - rt_X = rtarget[p_axis] - center_P, - rt_Y = rtarget[q_axis] - center_Q, - linear_travel = rtarget[l_axis] - current_position[l_axis], - extruder_travel = rtarget[E_AXIS] - current_position[E_AXIS]; + rt_X = cart[p_axis] - center_P, + rt_Y = cart[q_axis] - center_Q, + linear_travel = cart[l_axis] - current_position[l_axis], + extruder_travel = cart[E_AXIS] - current_position[E_AXIS]; // CCW angle of rotation between position and target from the circle center. Only one atan2() trig computation required. float angular_travel = ATAN2(r_P * rt_Y - r_Q * rt_X, r_P * rt_X + r_Q * rt_Y); @@ -77,7 +77,7 @@ void plan_arc( if (clockwise) angular_travel -= RADIANS(360); // Make a circle if the angular rotation is 0 and the target is current position - if (angular_travel == 0 && current_position[p_axis] == rtarget[p_axis] && current_position[q_axis] == rtarget[q_axis]) + if (angular_travel == 0 && current_position[p_axis] == cart[p_axis] && current_position[q_axis] == cart[q_axis]) angular_travel = RADIANS(360); const float mm_of_travel = HYPOT(angular_travel * radius, FABS(linear_travel)); @@ -177,7 +177,7 @@ void plan_arc( } // Ensure last segment arrives at target location. - planner.buffer_line_kinematic(rtarget, fr_mm_s, active_extruder); + planner.buffer_line_kinematic(cart, fr_mm_s, active_extruder); // As far as the parser is concerned, the position is now == target. In reality the // motion control system might still be processing the action and the real tool position diff --git a/Marlin/src/gcode/motion/G5.cpp b/Marlin/src/gcode/motion/G5.cpp index 78cc748be..684646941 100644 --- a/Marlin/src/gcode/motion/G5.cpp +++ b/Marlin/src/gcode/motion/G5.cpp @@ -27,7 +27,7 @@ #include "../../module/motion.h" #include "../../module/planner_bezier.h" -void plan_cubic_move(const float offset[4]) { +void plan_cubic_move(const float (&offset)[4]) { cubic_b_spline(current_position, destination, offset, MMS_SCALED(feedrate_mm_s), active_extruder); // As far as the parser is concerned, the position is now == destination. In reality the @@ -62,7 +62,7 @@ void GcodeSuite::G5() { get_destination_from_command(); - const float offset[] = { + const float offset[4] = { parser.linearval('I'), parser.linearval('J'), parser.linearval('P'), diff --git a/Marlin/src/inc/Conditionals_post.h b/Marlin/src/inc/Conditionals_post.h index 5b169ad17..094773024 100644 --- a/Marlin/src/inc/Conditionals_post.h +++ b/Marlin/src/inc/Conditionals_post.h @@ -977,15 +977,15 @@ /** * Set granular options based on the specific type of leveling */ -#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) -#define HAS_ABL (OLDSCHOOL_ABL || ENABLED(AUTO_BED_LEVELING_UBL)) -#define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) -#define HAS_AUTOLEVEL (HAS_ABL && DISABLED(PROBE_MANUALLY)) -#define HAS_MESH (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING)) -#define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING) || UBL_DELTA) +#define UBL_SEGMENTED (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) +#define HAS_ABL (OLDSCHOOL_ABL || ENABLED(AUTO_BED_LEVELING_UBL)) +#define HAS_LEVELING (HAS_ABL || ENABLED(MESH_BED_LEVELING)) +#define HAS_AUTOLEVEL (HAS_ABL && DISABLED(PROBE_MANUALLY)) +#define HAS_MESH (ENABLED(AUTO_BED_LEVELING_BILINEAR) || ENABLED(AUTO_BED_LEVELING_UBL) || ENABLED(MESH_BED_LEVELING)) +#define PLANNER_LEVELING (OLDSCHOOL_ABL || ENABLED(MESH_BED_LEVELING)) #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)) diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 2c6a8d331..ba8c42375 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -603,7 +603,7 @@ static_assert(1 >= 0 #error "Delta probably shouldn't use Z_MIN_PROBE_ENDSTOP. Comment out this line to continue." #elif 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) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_DELTA + #elif ENABLED(ENABLE_LEVELING_FADE_HEIGHT) && DISABLED(AUTO_BED_LEVELING_BILINEAR) && !UBL_SEGMENTED #error "ENABLE_LEVELING_FADE_HEIGHT on DELTA requires AUTO_BED_LEVELING_BILINEAR or AUTO_BED_LEVELING_UBL." #elif ENABLED(DELTA_AUTO_CALIBRATION) && !(HAS_BED_PROBE || ENABLED(ULTIPANEL)) #error "DELTA_AUTO_CALIBRATION requires a probe or LCD Controller." @@ -1497,9 +1497,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/src/module/motion.cpp b/Marlin/src/module/motion.cpp index 1384cb9c5..93eabc436 100644 --- a/Marlin/src/module/motion.cpp +++ b/Marlin/src/module/motion.cpp @@ -264,7 +264,7 @@ void buffer_line_to_destination(const float fr_mm_s) { gcode.refresh_cmd_timeout(); - #if UBL_DELTA + #if UBL_SEGMENTED // ubl segmented line will do z-only moves in single segment ubl.prepare_segmented_line_to(destination, MMS_SCALED(fr_mm_s ? fr_mm_s : feedrate_mm_s)); #else @@ -495,7 +495,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, #endif -#if !UBL_DELTA +#if !UBL_SEGMENTED #if IS_KINEMATIC #if ENABLED(AUTO_BED_LEVELING_BILINEAR) @@ -517,13 +517,19 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, /** * Prepare a linear move in a DELTA or SCARA setup. * + * Called from prepare_move_to_destination as the + * default Delta/SCARA segmenter. + * * This calls planner.buffer_line several times, adding * small incremental moves for DELTA or SCARA. * * For Unified Bed Leveling (Delta or Segmented Cartesian) * the ubl.prepare_segmented_line_to method replaces this. + * + * For Auto Bed Leveling (Bilinear) with SEGMENT_LEVELED_MOVES + * this is replaced by segmented_line_to_destination below. */ - inline bool prepare_kinematic_move_to(float rtarget[XYZE]) { + inline bool prepare_kinematic_move_to(const float (&rtarget)[XYZE]) { // Get the top feedrate of the move in the XY plane const float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s); @@ -756,7 +762,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, } #endif // !IS_KINEMATIC -#endif // !UBL_DELTA +#endif // !UBL_SEGMENTED #if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE) bool extruder_duplication_enabled = false; // Used in Dual X mode 2 @@ -790,7 +796,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, * * Return true if current_position[] was set to destination[] */ - inline bool prepare_move_to_destination_dualx() { + inline bool dual_x_carriage_unpark() { if (active_extruder_parked) { switch (dual_x_carriage_mode) { case DXC_FULL_CONTROL_MODE: @@ -859,7 +865,7 @@ float soft_endstop_min[XYZ] = { X_MIN_BED, Y_MIN_BED, Z_MIN_POS }, break; } } - return prepare_move_to_destination_cartesian(); + return false; } #endif // DUAL_X_CARRIAGE @@ -900,13 +906,15 @@ void prepare_move_to_destination() { #endif // PREVENT_COLD_EXTRUSION || PREVENT_LENGTHY_EXTRUDE + #if ENABLED(DUAL_X_CARRIAGE) + if (dual_x_carriage_unpark()) return; + #endif + if ( - #if UBL_DELTA // Also works for CARTESIAN (smaller segments follow mesh more closely) + #if UBL_SEGMENTED ubl.prepare_segmented_line_to(destination, MMS_SCALED(feedrate_mm_s)) #elif IS_KINEMATIC prepare_kinematic_move_to(destination) - #elif ENABLED(DUAL_X_CARRIAGE) - prepare_move_to_destination_dualx() #else prepare_move_to_destination_cartesian() #endif diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 3bf6fa5a7..cf3a31605 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; @@ -616,7 +609,7 @@ void Planner::calculate_volumetric_multipliers() { #endif rz += ( - #if ENABLED(AUTO_BED_LEVELING_UBL) // UBL_DELTA + #if ENABLED(AUTO_BED_LEVELING_UBL) ubl.get_z_correction(rx, ry) * fade_scaling_factor #elif ENABLED(MESH_BED_LEVELING) mbl.get_z(rx, ry @@ -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 } @@ -1365,7 +1351,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const } // _buffer_steps() /** - * Planner::_buffer_line + * Planner::buffer_segment * * Add a new linear movement to the buffer in axis units. * @@ -1375,7 +1361,7 @@ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const * fr_mm_s - (target) speed of the move * extruder - target extruder */ -void Planner::_buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) { +void Planner::buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder) { // When changing extruders recalculate steps corresponding to the E position #if ENABLED(DISTINCT_E_FACTORS) if (last_extruder != extruder && axis_steps_per_mm[E_AXIS_N] != axis_steps_per_mm[E_AXIS + last_extruder]) { @@ -1394,7 +1380,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const }; /* <-- add a slash to enable - SERIAL_ECHOPAIR(" _buffer_line FR:", fr_mm_s); + SERIAL_ECHOPAIR(" buffer_segment FR:", fr_mm_s); #if IS_KINEMATIC SERIAL_ECHOPAIR(" A:", a); SERIAL_ECHOPAIR(" (", position[A_AXIS]); @@ -1441,7 +1427,7 @@ void Planner::_buffer_line(const float &a, const float &b, const float &c, const stepper.wake_up(); -} // _buffer_line() +} // buffer_segment() /** * Directly set the planner XYZ position (and stepper positions) @@ -1466,18 +1452,18 @@ void Planner::_set_position_mm(const float &a, const float &b, const float &c, c ZERO(previous_speed); } -void Planner::set_position_mm_kinematic(const float position[NUM_AXIS]) { +void Planner::set_position_mm_kinematic(const float (&cart)[XYZE]) { #if PLANNER_LEVELING - float lpos[XYZ] = { position[X_AXIS], position[Y_AXIS], position[Z_AXIS] }; - apply_leveling(lpos); + float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; + apply_leveling(raw); #else - const float * const lpos = position; + const float (&raw)[XYZE] = cart; #endif #if IS_KINEMATIC - inverse_kinematics(lpos); - _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], position[E_AXIS]); + inverse_kinematics(raw); + _set_position_mm(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS]); #else - _set_position_mm(lpos[X_AXIS], lpos[Y_AXIS], lpos[Z_AXIS], position[E_AXIS]); + _set_position_mm(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS]); #endif } diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index cb0315c76..6134f5765 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -146,7 +146,7 @@ class Planner { * head!=tail : blocks are in the buffer * head==(tail-1)%size : the buffer is full * - * Writer of head is Planner::_buffer_line(). + * Writer of head is Planner::buffer_segment(). * Reader of tail is Stepper::isr(). Always consider tail busy / read-only */ static block_t block_buffer[BLOCK_BUFFER_SIZE]; @@ -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 @@ -356,7 +380,7 @@ class Planner { * as it will be given to the planner and steppers. */ static void apply_leveling(float &rx, float &ry, float &rz); - static void apply_leveling(float raw[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); } + static void apply_leveling(float (&raw)[XYZ]) { apply_leveling(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS]); } static void unapply_leveling(float raw[XYZ]); #else @@ -379,7 +403,7 @@ class Planner { static void _buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder); /** - * Planner::_buffer_line + * Planner::buffer_segment * * Add a new linear movement to the buffer in axis units. * @@ -389,7 +413,7 @@ class Planner { * fr_mm_s - (target) speed of the move * extruder - target extruder */ - static void _buffer_line(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder); + static void buffer_segment(const float &a, const float &b, const float &c, const float &e, const float &fr_mm_s, const uint8_t extruder); static void _set_position_mm(const float &a, const float &b, const float &c, const float &e); @@ -409,7 +433,7 @@ class Planner { #if PLANNER_LEVELING && IS_CARTESIAN apply_leveling(rx, ry, rz); #endif - _buffer_line(rx, ry, rz, e, fr_mm_s, extruder); + buffer_segment(rx, ry, rz, e, fr_mm_s, extruder); } /** @@ -421,18 +445,18 @@ class Planner { * fr_mm_s - (target) speed of the move (mm/s) * extruder - target extruder */ - FORCE_INLINE static void buffer_line_kinematic(const float cart[XYZE], const float &fr_mm_s, const uint8_t extruder) { + FORCE_INLINE static void buffer_line_kinematic(const float (&cart)[XYZE], const float &fr_mm_s, const uint8_t extruder) { #if PLANNER_LEVELING float raw[XYZ] = { cart[X_AXIS], cart[Y_AXIS], cart[Z_AXIS] }; apply_leveling(raw); #else - const float * const raw = cart; + const float (&raw)[XYZE] = cart; #endif #if IS_KINEMATIC inverse_kinematics(raw); - _buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder); + buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], cart[E_AXIS], fr_mm_s, extruder); #else - _buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder); + buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], cart[E_AXIS], fr_mm_s, extruder); #endif } @@ -451,7 +475,7 @@ class Planner { #endif _set_position_mm(rx, ry, rz, e); } - static void set_position_mm_kinematic(const float position[NUM_AXIS]); + static void set_position_mm_kinematic(const float (&cart)[XYZE]); static void set_position_mm(const AxisEnum axis, const float &v); FORCE_INLINE static void set_z_position_mm(const float &z) { set_position_mm(Z_AXIS, z); } FORCE_INLINE static void set_e_position_mm(const float &e) { set_position_mm(AxisEnum(E_AXIS), e); } diff --git a/Marlin/src/module/probe.cpp b/Marlin/src/module/probe.cpp index a8c6e32d1..8b42b1c99 100644 --- a/Marlin/src/module/probe.cpp +++ b/Marlin/src/module/probe.cpp @@ -107,7 +107,7 @@ inline void do_probe_raise(const float z_raise) { #elif ENABLED(Z_PROBE_ALLEN_KEY) - FORCE_INLINE void do_blocking_move_to(const float raw[XYZ], const float &fr_mm_s) { + FORCE_INLINE void do_blocking_move_to(const float (&raw)[XYZ], const float &fr_mm_s) { do_blocking_move_to(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], fr_mm_s); } diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index cbb227d93..c8e4203e8 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1193,7 +1193,7 @@ void Stepper::set_e_position(const long &e) { /** * Get a stepper's position in steps. */ -long Stepper::position(AxisEnum axis) { +long Stepper::position(const AxisEnum axis) { CRITICAL_SECTION_START; const long count_pos = count_position[axis]; CRITICAL_SECTION_END; @@ -1204,7 +1204,7 @@ long Stepper::position(AxisEnum axis) { * Get an axis position according to stepper position(s) * For CORE machines apply translation from ABC to XYZ. */ -float Stepper::get_axis_position_mm(AxisEnum axis) { +float Stepper::get_axis_position_mm(const AxisEnum axis) { float axis_steps; #if IS_CORE // Requesting one of the "core" axes? @@ -1242,7 +1242,7 @@ void Stepper::quick_stop() { #endif } -void Stepper::endstop_triggered(AxisEnum axis) { +void Stepper::endstop_triggered(const AxisEnum axis) { #if IS_CORE diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index b1e73bf05..8b27524d8 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -183,7 +183,7 @@ class Stepper { // // Get the position of a stepper, in steps // - static long position(AxisEnum axis); + static long position(const AxisEnum axis); // // Report the positions of the steppers, in steps @@ -193,13 +193,13 @@ class Stepper { // // Get the position (mm) of an axis based on stepper position(s) // - static float get_axis_position_mm(AxisEnum axis); + static float get_axis_position_mm(const AxisEnum axis); // // SCARA AB axes are in degrees, not mm // #if IS_SCARA - FORCE_INLINE static float get_axis_position_degrees(AxisEnum axis) { return get_axis_position_mm(axis); } + FORCE_INLINE static float get_axis_position_degrees(const AxisEnum axis) { return get_axis_position_mm(axis); } #endif // @@ -221,7 +221,7 @@ class Stepper { // // The direction of a single motor // - FORCE_INLINE static bool motor_direction(AxisEnum axis) { return TEST(last_direction_bits, axis); } + FORCE_INLINE static bool motor_direction(const AxisEnum axis) { return TEST(last_direction_bits, axis); } #if HAS_DIGIPOTSS || HAS_MOTOR_CURRENT_PWM static void digitalPotWrite(const int16_t address, const int16_t value); @@ -263,12 +263,12 @@ class Stepper { // // Handle a triggered endstop // - static void endstop_triggered(AxisEnum axis); + static void endstop_triggered(const AxisEnum axis); // // Triggered position of an axis in mm (not core-savvy) // - FORCE_INLINE static float triggered_position_mm(AxisEnum axis) { + FORCE_INLINE static float triggered_position_mm(const AxisEnum axis) { return endstops_trigsteps[axis] * planner.steps_to_mm[axis]; }