From d568e586b7f7b14f841bb59203e626d8941b6e18 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Mon, 11 Dec 2017 02:30:27 -0600 Subject: [PATCH] Fix up ubl_motion indentation --- .../src/feature/bedlevel/ubl/ubl_motion.cpp | 360 +++++++++--------- 1 file changed, 180 insertions(+), 180 deletions(-) diff --git a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp index 98d979e6ea..4bc106976d 100644 --- a/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp +++ b/Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp @@ -23,23 +23,23 @@ #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 - #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 @@ -409,219 +409,219 @@ #else // UBL_SEGMENTED - #if IS_SCARA // scale the feed rate from mm/s to degrees/s - static float scara_feed_factor, scara_oldA, scara_oldB; + #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 - // 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. + #if ENABLED(DELTA) // apply delta inverse_kinematics - inline void _O2 ubl_buffer_segment_raw(const float (&in_raw)[XYZE], const float &fr) { + DELTA_RAW_IK(); + planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder); - #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 + #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) - #if ENABLED(DELTA) // apply delta inverse_kinematics + inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE] + // should move the feedrate scaling to scara inverse_kinematics - DELTA_RAW_IK(); - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], fr, active_extruder); + 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; - #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) + planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder); - inverse_kinematics(raw); // this writes delta[ABC] from raw[XYZE] - // should move the feedrate scaling to scara inverse_kinematics + #else // CARTESIAN - 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(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder); - planner.buffer_segment(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], in_raw[E_AXIS], s_feedrate, active_extruder); + #endif + } - #else // CARTESIAN + #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 - planner.buffer_segment(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], in_raw[E_AXIS], fr, active_extruder); + /** + * 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). + */ - #endif - } + bool _O2 unified_bed_leveling::prepare_segmented_line_to(const float (&rtarget)[XYZE], const float &feedrate) { - #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 + 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 - /** - * 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). - */ + NOLESS(segments, 1); // must have at least one segment + const float inv_segments = 1.0 / segments; // divide once, multiply thereafter - 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(); + #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(); + } - // Otherwise perform per-segment leveling + // 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 + #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]; + // increment to first segment destination + LOOP_XYZE(i) raw[i] += diff[i]; - for(;;) { // for each mesh cell encountered during the move + 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. + // 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)); + 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); + 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); + 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 + 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 + 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; + 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) + 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) + 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 + 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_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) + // 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. + // 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 + 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 + for(;;) { // for all segments within this mesh cell - if (--segments == 0) // if this is last segment, use rtarget for exact - COPY(raw, rtarget); + 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_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; + 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() + if (segments == 0) // done with last segment + return false; // did not set_current_from_destination() - LOOP_XYZE(i) raw[i] += diff[i]; + LOOP_XYZE(i) raw[i] += diff[i]; - cx += diff[X_AXIS]; - cy += diff[Y_AXIS]; + 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; + 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. + // 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 + 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 - } + } // segment loop + } // cell loop + } #endif // UBL_SEGMENTED