From 8b7c274db5c18df5397c872657173133a593395c Mon Sep 17 00:00:00 2001
From: Scott Lahteine <github@thinkyhead.com>
Date: Fri, 1 Dec 2017 23:21:02 -0600
Subject: [PATCH] Comment/cleanup motion code
---
Marlin/Marlin.h | 28 +++++-
Marlin/Marlin_main.cpp | 112 ++++++++++------------
Marlin/planner.h | 6 +-
Marlin/stepper.cpp | 3 +-
Marlin/ubl_motion.cpp | 211 +++++++++++++++--------------------------
5 files changed, 160 insertions(+), 200 deletions(-)
diff --git a/Marlin/Marlin.h b/Marlin/Marlin.h
index 9035f08b4a8..78b9c32cee2 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 a2c765d1c58..2c586d40235 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 75983922d38..6d658b2a883 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 d6cc306ac50..bf99b4afda6 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 3bf679d2cb4..35127c04658 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 ));
+ DELTA_RAW_IK();
+ planner._buffer_line(delta[A_AXIS], delta[B_AXIS], delta[C_AXIS], raw[E_AXIS], fr, active_extruder);
- 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 ));
+ #elif IS_SCARA // apply scara inverse_kinematics (should be changed to save raw->logical->raw)
- 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 ));
-
- planner._buffer_line(delta_A, delta_B, delta_C, e, fr, active_extruder);
-
- #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
-