diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h index 9035f08b4..78b9c32ce 100644 --- a/Marlin/Marlin.h +++ b/Marlin/Marlin.h @@ -301,12 +301,38 @@ void report_current_position(); extern float delta_height, delta_endstop_adj[ABC], delta_radius, + delta_tower_angle_trim[ABC], + delta_tower[ABC][2], delta_diagonal_rod, delta_calibration_radius, + delta_diagonal_rod_2_tower[ABC], delta_segments_per_second, - delta_tower_angle_trim[ABC], delta_clip_start_height; + void recalc_delta_settings(); + float delta_safe_distance_from_top(); + + #if ENABLED(DELTA_FAST_SQRT) + float Q_rsqrt(const float number); + #define _SQRT(n) (1.0f / Q_rsqrt(n)) + #else + #define _SQRT(n) SQRT(n) + #endif + + // Macro to obtain the Z position of an individual tower + #define DELTA_Z(T) raw[Z_AXIS] + _SQRT( \ + delta_diagonal_rod_2_tower[T] - HYPOT2( \ + delta_tower[T][X_AXIS] - raw[X_AXIS], \ + delta_tower[T][Y_AXIS] - raw[Y_AXIS] \ + ) \ + ) + + #define DELTA_RAW_IK() do { \ + delta[A_AXIS] = DELTA_Z(A_AXIS); \ + delta[B_AXIS] = DELTA_Z(B_AXIS); \ + delta[C_AXIS] = DELTA_Z(C_AXIS); \ + }while(0) + #elif IS_SCARA void forward_kinematics_SCARA(const float &a, const float &b); #endif diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index a2c765d1c..2c586d402 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -12258,7 +12258,7 @@ void ok_to_send() { * Fast inverse sqrt from Quake III Arena * See: https://en.wikipedia.org/wiki/Fast_inverse_square_root */ - float Q_rsqrt(float number) { + float Q_rsqrt(const float number) { long i; float x2, y; const float threehalfs = 1.5f; @@ -12272,12 +12272,6 @@ void ok_to_send() { return y; } - #define _SQRT(n) (1.0f / Q_rsqrt(n)) - - #else - - #define _SQRT(n) SQRT(n) - #endif /** @@ -12299,20 +12293,6 @@ void ok_to_send() { * (see above) */ - // Macro to obtain the Z position of an individual tower - #define DELTA_Z(T) raw[Z_AXIS] + _SQRT( \ - delta_diagonal_rod_2_tower[T] - HYPOT2( \ - delta_tower[T][X_AXIS] - raw[X_AXIS], \ - delta_tower[T][Y_AXIS] - raw[Y_AXIS] \ - ) \ - ) - - #define DELTA_RAW_IK() do { \ - delta[A_AXIS] = DELTA_Z(A_AXIS); \ - delta[B_AXIS] = DELTA_Z(B_AXIS); \ - delta[C_AXIS] = DELTA_Z(C_AXIS); \ - }while(0) - #define DELTA_DEBUG() do { \ SERIAL_ECHOPAIR("cartesian X:", raw[X_AXIS]); \ SERIAL_ECHOPAIR(" Y:", raw[Y_AXIS]); \ @@ -12367,46 +12347,53 @@ void ok_to_send() { */ void forward_kinematics_DELTA(float z1, float z2, float z3) { // Create a vector in old coordinates along x axis of new coordinate - float p12[3] = { delta_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z2 - z1 }; + const float p12[] = { + delta_tower[B_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], + delta_tower[B_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], + z2 - z1 + }, // Get the Magnitude of vector. - float d = SQRT( sq(p12[0]) + sq(p12[1]) + sq(p12[2]) ); + d = SQRT(sq(p12[0]) + sq(p12[1]) + sq(p12[2])), // Create unit vector by dividing by magnitude. - float ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d }; + ex[3] = { p12[0] / d, p12[1] / d, p12[2] / d }, // Get the vector from the origin of the new system to the third point. - float p13[3] = { delta_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], z3 - z1 }; + p13[3] = { + delta_tower[C_AXIS][X_AXIS] - delta_tower[A_AXIS][X_AXIS], + delta_tower[C_AXIS][Y_AXIS] - delta_tower[A_AXIS][Y_AXIS], + z3 - z1 + }, // Use the dot product to find the component of this vector on the X axis. - float i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2]; + i = ex[0] * p13[0] + ex[1] * p13[1] + ex[2] * p13[2], // Create a vector along the x axis that represents the x component of p13. - float iex[3] = { ex[0] * i, ex[1] * i, ex[2] * i }; + iex[] = { ex[0] * i, ex[1] * i, ex[2] * i }; // Subtract the X component from the original vector leaving only Y. We use the // variable that will be the unit vector after we scale it. float ey[3] = { p13[0] - iex[0], p13[1] - iex[1], p13[2] - iex[2] }; // The magnitude of Y component - float j = SQRT( sq(ey[0]) + sq(ey[1]) + sq(ey[2]) ); + const float j = SQRT(sq(ey[0]) + sq(ey[1]) + sq(ey[2])); // Convert to a unit vector ey[0] /= j; ey[1] /= j; ey[2] /= j; // The cross product of the unit x and y is the unit z // float[] ez = vectorCrossProd(ex, ey); - float ez[3] = { + const float ez[3] = { ex[1] * ey[2] - ex[2] * ey[1], ex[2] * ey[0] - ex[0] * ey[2], ex[0] * ey[1] - ex[1] * ey[0] - }; - + }, // We now have the d, i and j values defined in Wikipedia. // Plug them into the equations defined in Wikipedia for Xnew, Ynew and Znew - float Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + sq(d)) / (d * 2), - Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + HYPOT2(i, j)) / 2 - i * Xnew) / j, - Znew = SQRT(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew)); + Xnew = (delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[B_AXIS] + sq(d)) / (d * 2), + Ynew = ((delta_diagonal_rod_2_tower[A_AXIS] - delta_diagonal_rod_2_tower[C_AXIS] + HYPOT2(i, j)) / 2 - i * Xnew) / j, + Znew = SQRT(delta_diagonal_rod_2_tower[A_AXIS] - HYPOT2(Xnew, Ynew)); // Start from the origin of the old coordinates and add vectors in the // old coords that represent the Xnew, Ynew and Znew to find the point @@ -12478,7 +12465,7 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { * small incremental moves. This allows the planner to * apply more detailed bed leveling to the full move. */ - inline void segmented_line_to_destination(const float fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) { + inline void segmented_line_to_destination(const float &fr_mm_s, const float segment_size=LEVELED_SEGMENT_LENGTH) { const float xdiff = destination[X_AXIS] - current_position[X_AXIS], ydiff = destination[Y_AXIS] - current_position[Y_AXIS]; @@ -12517,16 +12504,12 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { // SERIAL_ECHOPAIR("mm=", cartesian_mm); // SERIAL_ECHOLNPAIR(" segments=", segments); - // Drop one segment so the last move is to the exact target. - // If there's only 1 segment, loops will be skipped entirely. - --segments; - // Get the raw current position as starting point float raw[XYZE]; COPY(raw, current_position); // Calculate and execute the segments - for (uint16_t s = segments + 1; --s;) { + while (--segments) { static millis_t next_idle_ms = millis() + 200UL; thermalManager.manage_heater(); // This returns immediately if not really needed. if (ELAPSED(millis(), next_idle_ms)) { @@ -12548,7 +12531,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { * Prepare a mesh-leveled linear move in a Cartesian setup, * splitting the move where it crosses mesh borders. */ - void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits = 0xFF, uint8_t y_splits = 0xFF) { + void mesh_line_to_destination(const float fr_mm_s, uint8_t x_splits=0xFF, uint8_t y_splits=0xFF) { + // Get current and destination cells for this line int cx1 = mbl.cell_index_x(current_position[X_AXIS]), cy1 = mbl.cell_index_y(current_position[Y_AXIS]), cx2 = mbl.cell_index_x(destination[X_AXIS]), @@ -12558,8 +12542,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { NOMORE(cx2, GRID_MAX_POINTS_X - 2); NOMORE(cy2, GRID_MAX_POINTS_Y - 2); + // Start and end in the same cell? No split needed. if (cx1 == cx2 && cy1 == cy2) { - // Start and end on same mesh square buffer_line_to_destination(fr_mm_s); set_current_from_destination(); return; @@ -12568,25 +12552,30 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { #define MBL_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist) float normalized_dist, end[XYZE]; - - // Split at the left/front border of the right/top square const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); + + // Crosses on the X and not already split on this X? + // The x_splits flags are insurance against rounding errors. if (cx2 != cx1 && TEST(x_splits, gcx)) { + // Split on the X grid line + CBI(x_splits, gcx); COPY(end, destination); destination[X_AXIS] = mbl.index_to_xpos[gcx]; normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]); destination[Y_AXIS] = MBL_SEGMENT_END(Y); - CBI(x_splits, gcx); } + // Crosses on the Y and not already split on this Y? else if (cy2 != cy1 && TEST(y_splits, gcy)) { + // Split on the Y grid line + CBI(y_splits, gcy); COPY(end, destination); destination[Y_AXIS] = mbl.index_to_ypos[gcy]; normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]); destination[X_AXIS] = MBL_SEGMENT_END(X); - CBI(y_splits, gcy); } else { - // Already split on a border + // Must already have been split on these border(s) + // This should be a rare case. buffer_line_to_destination(fr_mm_s); set_current_from_destination(); return; @@ -12611,7 +12600,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { * Prepare a bilinear-leveled linear move on Cartesian, * splitting the move where it crosses grid borders. */ - void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits = 0xFFFF, uint16_t y_splits = 0xFFFF) { + void bilinear_line_to_destination(const float fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF) { + // Get current and destination cells for this line int cx1 = CELL_INDEX(X, current_position[X_AXIS]), cy1 = CELL_INDEX(Y, current_position[Y_AXIS]), cx2 = CELL_INDEX(X, destination[X_AXIS]), @@ -12621,8 +12611,8 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { cx2 = constrain(cx2, 0, ABL_BG_POINTS_X - 2); cy2 = constrain(cy2, 0, ABL_BG_POINTS_Y - 2); + // Start and end in the same cell? No split needed. if (cx1 == cx2 && cy1 == cy2) { - // Start and end on same mesh square buffer_line_to_destination(fr_mm_s); set_current_from_destination(); return; @@ -12631,25 +12621,30 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { #define LINE_SEGMENT_END(A) (current_position[A ##_AXIS] + (destination[A ##_AXIS] - current_position[A ##_AXIS]) * normalized_dist) float normalized_dist, end[XYZE]; - - // Split at the left/front border of the right/top square const int8_t gcx = max(cx1, cx2), gcy = max(cy1, cy2); + + // Crosses on the X and not already split on this X? + // The x_splits flags are insurance against rounding errors. if (cx2 != cx1 && TEST(x_splits, gcx)) { + // Split on the X grid line + CBI(x_splits, gcx); COPY(end, destination); destination[X_AXIS] = bilinear_start[X_AXIS] + ABL_BG_SPACING(X_AXIS) * gcx; normalized_dist = (destination[X_AXIS] - current_position[X_AXIS]) / (end[X_AXIS] - current_position[X_AXIS]); destination[Y_AXIS] = LINE_SEGMENT_END(Y); - CBI(x_splits, gcx); } + // Crosses on the Y and not already split on this Y? else if (cy2 != cy1 && TEST(y_splits, gcy)) { + // Split on the Y grid line + CBI(y_splits, gcy); COPY(end, destination); destination[Y_AXIS] = bilinear_start[Y_AXIS] + ABL_BG_SPACING(Y_AXIS) * gcy; normalized_dist = (destination[Y_AXIS] - current_position[Y_AXIS]) / (end[Y_AXIS] - current_position[Y_AXIS]); destination[X_AXIS] = LINE_SEGMENT_END(X); - CBI(y_splits, gcy); } else { - // Already split on a border + // Must already have been split on these border(s) + // This should be a rare case. buffer_line_to_destination(fr_mm_s); set_current_from_destination(); return; @@ -12745,16 +12740,13 @@ void set_current_from_steppers_for_axis(const AxisEnum axis) { oldB = stepper.get_axis_position_degrees(B_AXIS); #endif - // Get the raw current position as starting point + // Get the current position as starting point float raw[XYZE]; COPY(raw, current_position); - // Drop one segment so the last move is to the exact target. - // If there's only 1 segment, loops will be skipped entirely. - --segments; // Calculate and execute the segments - for (uint16_t s = segments + 1; --s;) { + while (--segments) { static millis_t next_idle_ms = millis() + 200UL; thermalManager.manage_heater(); // This returns immediately if not really needed. @@ -13033,7 +13025,7 @@ void prepare_move_to_destination() { if (mm_of_travel < 0.001) return; uint16_t segments = FLOOR(mm_of_travel / (MM_PER_ARC_SEGMENT)); - if (segments == 0) segments = 1; + NOLESS(segments, 1); /** * Vector rotation by transformation matrix: r is the original vector, r_T is the rotated vector, diff --git a/Marlin/planner.h b/Marlin/planner.h index 75983922d..6d658b2a8 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -140,7 +140,7 @@ class Planner { static uint8_t last_extruder; // Respond to extruder change #endif - static int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder + static int16_t flow_percentage[EXTRUDERS]; // Extrusion factor for each extruder static float e_factor[EXTRUDERS], // The flow percentage and volumetric multiplier combine to scale E movement filament_size[EXTRUDERS], // diameter of filament (in millimeters), typically around 1.75 or 2.85, 0 disables the volumetric calculations for the extruder @@ -501,8 +501,8 @@ class Planner { /** * Get the index of the next / previous block in the ring buffer */ - static int8_t next_block_index(int8_t block_index) { return BLOCK_MOD(block_index + 1); } - static int8_t prev_block_index(int8_t block_index) { return BLOCK_MOD(block_index - 1); } + static int8_t next_block_index(const int8_t block_index) { return BLOCK_MOD(block_index + 1); } + static int8_t prev_block_index(const int8_t block_index) { return BLOCK_MOD(block_index - 1); } /** * Calculate the distance (not time) it takes to accelerate diff --git a/Marlin/stepper.cpp b/Marlin/stepper.cpp index d6cc306ac..bf99b4afd 100644 --- a/Marlin/stepper.cpp +++ b/Marlin/stepper.cpp @@ -443,8 +443,7 @@ void Stepper::isr() { // If there is no current block, attempt to pop one from the buffer if (!current_block) { // Anything in the buffer? - current_block = planner.get_current_block(); - if (current_block) { + if ((current_block = planner.get_current_block())) { trapezoid_generator_reset(); // Initialize Bresenham counters to 1/2 the ceiling diff --git a/Marlin/ubl_motion.cpp b/Marlin/ubl_motion.cpp index 3bf679d2c..35127c046 100644 --- a/Marlin/ubl_motion.cpp +++ b/Marlin/ubl_motion.cpp @@ -38,25 +38,6 @@ extern void set_current_from_destination(); #endif - #if ENABLED(DELTA) - - extern float delta[ABC]; - - extern float delta_endstop_adj[ABC], - delta_radius, - delta_tower_angle_trim[ABC], - delta_tower[ABC][2], - delta_diagonal_rod, - delta_calibration_radius, - delta_diagonal_rod_2_tower[ABC], - delta_segments_per_second, - delta_clip_start_height; - - extern float delta_safe_distance_from_top(); - - #endif - - static void debug_echo_axis(const AxisEnum axis) { if (current_position[axis] == destination[axis]) SERIAL_ECHOPGM("-------------"); @@ -268,9 +249,9 @@ * else, we know the next X is the same so we can recover and continue! * Calculate X at the next Y mesh line */ - const float x = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m; + const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m; - float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi, current_yi) + float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi) * planner.fade_scaling_factor_for_z(end[Z_AXIS]); /** @@ -282,7 +263,7 @@ */ if (isnan(z0)) z0 = 0.0; - const float y = mesh_index_to_ypos(current_yi); + const float ry = mesh_index_to_ypos(current_yi); /** * Without this check, it is possible for the algorithm to generate a zero length move in the case @@ -290,9 +271,9 @@ * 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. */ - if (y != start[Y_AXIS]) { + if (ry != start[Y_AXIS]) { if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS]; + on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS]; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; } @@ -301,7 +282,7 @@ z_position = end[Z_AXIS]; } - planner._buffer_line(x, y, z_position + z0, e_position, feed_rate, extruder); + planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder); } //else printf("FIRST MOVE PRUNED "); } @@ -332,9 +313,9 @@ while (current_xi != cell_dest_xi + left_flag) { current_xi += dxi; const float next_mesh_line_x = mesh_index_to_xpos(current_xi), - y = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line + ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line - float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi, current_yi) + float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi) * planner.fade_scaling_factor_for_z(end[Z_AXIS]); /** @@ -346,7 +327,7 @@ */ if (isnan(z0)) z0 = 0.0; - const float x = mesh_index_to_xpos(current_xi); + const float rx = mesh_index_to_xpos(current_xi); /** * Without this check, it is possible for the algorithm to generate a zero length move in the case @@ -354,9 +335,9 @@ * 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. */ - if (x != start[X_AXIS]) { + if (rx != start[X_AXIS]) { if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? x - start[X_AXIS] : y - start[Y_AXIS]; + on_axis_distance = use_x_dist ? rx - start[X_AXIS] : ry - start[Y_AXIS]; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; } @@ -365,7 +346,7 @@ z_position = end[Z_AXIS]; } - planner._buffer_line(x, y, z_position + z0, e_position, feed_rate, extruder); + planner._buffer_line(rx, ry, z_position + z0, e_position, feed_rate, extruder); } //else printf("FIRST MOVE PRUNED "); } @@ -398,15 +379,15 @@ const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi), next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi), - y = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line - x = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line - // (No need to worry about m being zero. - // If that was the case, it was already detected - // as a vertical line move above.) + ry = m * next_mesh_line_x + c, // Calculate Y at the next X mesh line + rx = (next_mesh_line_y - c) / m; // Calculate X at the next Y mesh line + // (No need to worry about m being zero. + // If that was the case, it was already detected + // as a vertical line move above.) - if (left_flag == (x > next_mesh_line_x)) { // Check if we hit the Y line first + if (left_flag == (rx > next_mesh_line_x)) { // Check if we hit the Y line first // Yes! Crossing a Y Mesh Line next - float z0 = z_correction_for_x_on_horizontal_mesh_line(x, current_xi - left_flag, current_yi + dyi) + float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi - left_flag, current_yi + dyi) * planner.fade_scaling_factor_for_z(end[Z_AXIS]); /** @@ -419,7 +400,7 @@ if (isnan(z0)) z0 = 0.0; if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? x - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS]; + on_axis_distance = use_x_dist ? rx - start[X_AXIS] : next_mesh_line_y - start[Y_AXIS]; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; } @@ -427,13 +408,13 @@ e_position = end[E_AXIS]; z_position = end[Z_AXIS]; } - planner._buffer_line(x, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); + planner._buffer_line(rx, next_mesh_line_y, z_position + z0, e_position, feed_rate, extruder); current_yi += dyi; yi_cnt--; } else { // Yes! Crossing a X Mesh Line next - float z0 = z_correction_for_y_on_vertical_mesh_line(y, current_xi + dxi, current_yi - down_flag) + float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi + dxi, current_yi - down_flag) * planner.fade_scaling_factor_for_z(end[Z_AXIS]); /** @@ -446,7 +427,7 @@ if (isnan(z0)) z0 = 0.0; if (!inf_normalized_flag) { - on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : y - start[Y_AXIS]; + on_axis_distance = use_x_dist ? next_mesh_line_x - start[X_AXIS] : ry - start[Y_AXIS]; e_position = start[E_AXIS] + on_axis_distance * e_normalized_dist; z_position = start[Z_AXIS] + on_axis_distance * z_normalized_dist; } @@ -455,7 +436,7 @@ z_position = end[Z_AXIS]; } - planner._buffer_line(next_mesh_line_x, y, z_position + z0, e_position, feed_rate, extruder); + planner._buffer_line(next_mesh_line_x, ry, z_position + z0, e_position, feed_rate, extruder); current_xi += dxi; xi_cnt--; } @@ -489,29 +470,16 @@ // 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. - inline void _O2 ubl_buffer_segment_raw(const float &rx, const float &ry, const float rz, const float &e, const float &fr) { + inline void _O2 ubl_buffer_segment_raw(const float raw[XYZE], const float &fr) { #if ENABLED(DELTA) // apply delta inverse_kinematics - const float delta_A = rz + SQRT( delta_diagonal_rod_2_tower[A_AXIS] - - HYPOT2( delta_tower[A_AXIS][X_AXIS] - rx, - delta_tower[A_AXIS][Y_AXIS] - ry )); - - const float delta_B = rz + SQRT( delta_diagonal_rod_2_tower[B_AXIS] - - HYPOT2( delta_tower[B_AXIS][X_AXIS] - rx, - delta_tower[B_AXIS][Y_AXIS] - ry )); - - const float delta_C = rz + SQRT( delta_diagonal_rod_2_tower[C_AXIS] - - HYPOT2( delta_tower[C_AXIS][X_AXIS] - rx, - delta_tower[C_AXIS][Y_AXIS] - ry )); + DELTA_RAW_IK(); + planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder); - planner._buffer_line(delta_A, delta_B, delta_C, e, fr, active_extruder); + #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw) - #elif IS_SCARA // apply scara inverse_kinematics - - const float lseg[XYZ] = { rx, ry, rz }; - - inverse_kinematics(lseg); // this writes delta[ABC] from lseg[XYZ] + 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), @@ -520,14 +488,13 @@ 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], e, s_feedrate, active_extruder); + planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], s_feedrate, active_extruder); #else // CARTESIAN - planner._buffer_line(rx, ry, rz, e, fr, active_extruder); + planner._buffer_line(raw[X_AXIS], raw[Y_AXIS], raw[Z_AXIS], raw[E_AXIS], fr, active_extruder); #endif - } @@ -542,12 +509,14 @@ 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 tot_dx = rtarget[X_AXIS] - current_position[X_AXIS], - tot_dy = rtarget[Y_AXIS] - current_position[Y_AXIS], - tot_dz = rtarget[Z_AXIS] - current_position[Z_AXIS], - tot_de = rtarget[E_AXIS] - current_position[E_AXIS]; + 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(tot_dx, tot_dy); // total horizontal xy distance + 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 @@ -567,49 +536,30 @@ scara_oldB = stepper.get_axis_position_degrees(B_AXIS); #endif - const float seg_dx = tot_dx * inv_segments, - seg_dy = tot_dy * inv_segments, - seg_dz = tot_dz * inv_segments, - seg_de = tot_de * inv_segments; + 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 seg_rx = current_position[X_AXIS], - seg_ry = current_position[Y_AXIS], - seg_rz = current_position[Z_AXIS], - seg_le = current_position[E_AXIS]; - - const bool above_fade_height = ( - #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) - planner.z_fade_height != 0 && planner.z_fade_height < rtarget[Z_AXIS] - #else - false - #endif - ); + 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 - - do { - - if (--segments) { // not the last segment - seg_rx += seg_dx; - seg_ry += seg_dy; - seg_rz += seg_dz; - seg_le += seg_de; - } else { // last segment, use exact destination - seg_rx = rtarget[X_AXIS]; - seg_ry = rtarget[Y_AXIS]; - seg_rz = rtarget[Z_AXIS]; - seg_le = rtarget[E_AXIS]; - } - - ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz, seg_le, feedrate); - - } while (segments); - + 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(); } @@ -617,15 +567,10 @@ #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT) const float fade_scaling_factor = planner.fade_scaling_factor_for_z(rtarget[Z_AXIS]); - #else - constexpr float fade_scaling_factor = 1.0; #endif // increment to first segment destination - seg_rx += seg_dx; - seg_ry += seg_dy; - seg_rz += seg_dz; - seg_le += seg_de; + LOOP_XYZE(i) raw[i] += diff[i]; for(;;) { // for each mesh cell encountered during the move @@ -636,8 +581,8 @@ // 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 = (seg_rx - (MESH_MIN_X)) * (1.0 / (MESH_X_DIST)), - cell_yi = (seg_ry - (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); @@ -655,8 +600,8 @@ 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 = seg_rx - x0, // cell-relative x and y - cy = seg_ry - 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) @@ -674,36 +619,35 @@ // 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 * seg_dx, // per-segment adjustment to z_cxy0 - z_sxym = (z_xmy1 - z_xmy0) * (1.0 / (MESH_Y_DIST)) * seg_dx; // 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 - float z_cxcy = (z_cxy0 + z_cxym * cy) * fade_scaling_factor; // interpolated mesh z height along cx at cy, scaled for fade + 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 - seg_rx = rtarget[X_AXIS]; - seg_ry = rtarget[Y_AXIS]; - seg_rz = rtarget[Z_AXIS]; - seg_le = rtarget[E_AXIS]; - } + 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 + ; - ubl_buffer_segment_raw(seg_rx, seg_ry, seg_rz + z_cxcy, seg_le, feedrate); + 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() - seg_rx += seg_dx; - seg_ry += seg_dy; - seg_rz += seg_dz; - seg_le += seg_de; + LOOP_XYZE(i) raw[i] += diff[i]; - cx += seg_dx; - cy += seg_dy; + 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 + 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. @@ -718,4 +662,3 @@ #endif // UBL_DELTA #endif // AUTO_BED_LEVELING_UBL -