bissen21/Marlin_Firmware
1
0
Fork 0
Code Issues Pull Requests Projects Releases 4 Wiki Activity

Add custom types for position (#15204)

Browse Source
This commit is contained in:
Scott Lahteine
2019-09-29 04:25:39 -05:00
committed by GitHub
parent 43d6e9fa43
commit 50e4545255
227 changed files with 3147 additions and 3264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
Marlin/src/feature/I2CPositionEncoder.cpp
View File

@ -326,25 +326,23 @@ bool I2CPositionEncoder::test_axis() {
//only works on XYZ cartesian machines for the time being
if (!(encoderAxis == X_AXIS || encoderAxis == Y_AXIS || encoderAxis == Z_AXIS)) return false;
float startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 };
const float startPosition = soft_endstop[encoderAxis].min + 10,
endPosition = soft_endstop[encoderAxis].max - 10;
const float startPosition = soft_endstop.min[encoderAxis] + 10,
endPosition = soft_endstop.max[encoderAxis] - 10;
const feedRate_t fr_mm_s = FLOOR(MMM_TO_MMS((encoderAxis == Z_AXIS) ? HOMING_FEEDRATE_Z : HOMING_FEEDRATE_XY));
ec = false;
LOOP_XYZ(i) {
startCoord[i] = planner.get_axis_position_mm((AxisEnum)i);
endCoord[i] = planner.get_axis_position_mm((AxisEnum)i);
xyze_pos_t startCoord, endCoord;
LOOP_XYZ(a) {
startCoord[a] = planner.get_axis_position_mm((AxisEnum)a);
endCoord[a] = planner.get_axis_position_mm((AxisEnum)a);
}
startCoord[encoderAxis] = startPosition;
endCoord[encoderAxis] = endPosition;
planner.synchronize();
planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
planner.get_axis_position_mm(E_AXIS), fr_mm_s, 0);
startCoord.e = planner.get_axis_position_mm(E_AXIS);
planner.buffer_line(startCoord, fr_mm_s, 0);
planner.synchronize();
// if the module isn't currently trusted, wait until it is (or until it should be if things are working)
@ -355,8 +353,8 @@ bool I2CPositionEncoder::test_axis() {
}
if (trusted) { // if trusted, commence test
planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS],
planner.get_axis_position_mm(E_AXIS), fr_mm_s, 0);
endCoord.e = planner.get_axis_position_mm(E_AXIS);
planner.buffer_line(endCoord, fr_mm_s, 0);
planner.synchronize();
}
@ -376,8 +374,7 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
float old_steps_mm, new_steps_mm,
startDistance, endDistance,
travelDistance, travelledDistance, total = 0,
startCoord[NUM_AXIS] = { 0 }, endCoord[NUM_AXIS] = { 0 };
travelDistance, travelledDistance, total = 0;
int32_t startCount, stopCount;
@ -387,31 +384,31 @@ void I2CPositionEncoder::calibrate_steps_mm(const uint8_t iter) {
ec = false;
startDistance = 20;
endDistance = soft_endstop[encoderAxis].max - 20;
endDistance = soft_endstop.max[encoderAxis] - 20;
travelDistance = endDistance - startDistance;
xyze_pos_t startCoord, endCoord;
LOOP_XYZ(a) {
startCoord[a] = planner.get_axis_position_mm((AxisEnum)a);
endCoord[a] = planner.get_axis_position_mm((AxisEnum)a);
}
startCoord[encoderAxis] = startDistance;
endCoord[encoderAxis] = endDistance;
planner.synchronize();
LOOP_L_N(i, iter) {
planner.buffer_line(startCoord[X_AXIS], startCoord[Y_AXIS], startCoord[Z_AXIS],
planner.get_axis_position_mm(E_AXIS), fr_mm_s, 0);
startCoord.e = planner.get_axis_position_mm(E_AXIS);
planner.buffer_line(startCoord, fr_mm_s, 0);
planner.synchronize();
delay(250);
startCount = get_position();
//do_blocking_move_to(endCoord[X_AXIS],endCoord[Y_AXIS],endCoord[Z_AXIS]);
//do_blocking_move_to(endCoord);
planner.buffer_line(endCoord[X_AXIS], endCoord[Y_AXIS], endCoord[Z_AXIS],
planner.get_axis_position_mm(E_AXIS), fr_mm_s, 0);
endCoord.e = planner.get_axis_position_mm(E_AXIS);
planner.buffer_line(endCoord, fr_mm_s, 0);
planner.synchronize();
//Read encoder distance

2
Marlin/src/feature/I2CPositionEncoder.h
View File

@ -93,8 +93,6 @@
#define LOOP_PE(VAR) LOOP_L_N(VAR, I2CPE_ENCODER_CNT)
#define CHECK_IDX() do{ if (!WITHIN(idx, 0, I2CPE_ENCODER_CNT - 1)) return; }while(0)
extern const char axis_codes[XYZE];
typedef union {
volatile int32_t val = 0;
uint8_t bval[4];

24
Marlin/src/feature/backlash.cpp
View File

@ -31,9 +31,9 @@
#ifdef BACKLASH_DISTANCE_MM
#if ENABLED(BACKLASH_GCODE)
float Backlash::distance_mm[XYZ] = BACKLASH_DISTANCE_MM;
xyz_float_t Backlash::distance_mm = BACKLASH_DISTANCE_MM;
#else
const float Backlash::distance_mm[XYZ] = BACKLASH_DISTANCE_MM;
const xyz_float_t Backlash::distance_mm = BACKLASH_DISTANCE_MM;
#endif
#endif
@ -45,8 +45,8 @@
#endif
#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
float Backlash::measured_mm[XYZ] = { 0 };
uint8_t Backlash::measured_count[XYZ] = { 0 };
xyz_float_t Backlash::measured_mm{0};
xyz_uint8_t Backlash::measured_count{0};
#endif
Backlash backlash;
@ -80,12 +80,12 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
// Residual error carried forward across multiple segments, so correction can be applied
// to segments where there is no direction change.
static int32_t residual_error[XYZ] = { 0 };
static xyz_long_t residual_error{0};
#else
// No direction change, no correction.
if (!changed_dir) return;
// No leftover residual error from segment to segment
int32_t residual_error[XYZ] = { 0 };
xyz_long_t residual_error{0};
#endif
const float f_corr = float(correction) / 255.0f;
@ -131,15 +131,15 @@ void Backlash::add_correction_steps(const int32_t &da, const int32_t &db, const
// Measure Z backlash by raising nozzle in increments until probe deactivates
void Backlash::measure_with_probe() {
if (measured_count[Z_AXIS] == 255) return;
if (measured_count.z == 255) return;
float start_height = current_position[Z_AXIS];
while (current_position[Z_AXIS] < (start_height + BACKLASH_MEASUREMENT_LIMIT) && TEST_PROBE_PIN)
do_blocking_move_to_z(current_position[Z_AXIS] + BACKLASH_MEASUREMENT_RESOLUTION, MMM_TO_MMS(BACKLASH_MEASUREMENT_FEEDRATE));
const float start_height = current_position.z;
while (current_position.z < (start_height + BACKLASH_MEASUREMENT_LIMIT) && TEST_PROBE_PIN)
do_blocking_move_to_z(current_position.z + BACKLASH_MEASUREMENT_RESOLUTION, MMM_TO_MMS(BACKLASH_MEASUREMENT_FEEDRATE));
// The backlash from all probe points is averaged, so count the number of measurements
measured_mm[Z_AXIS] += current_position[Z_AXIS] - start_height;
measured_count[Z_AXIS]++;
measured_mm.z += current_position.z - start_height;
measured_count.z++;
}
#endif

8
Marlin/src/feature/backlash.h
View File

@ -29,7 +29,7 @@ constexpr uint8_t all_on = 0xFF, all_off = 0x00;
class Backlash {
public:
#if ENABLED(BACKLASH_GCODE)
static float distance_mm[XYZ];
static xyz_float_t distance_mm;
static uint8_t correction;
#ifdef BACKLASH_SMOOTHING_MM
static float smoothing_mm;
@ -39,7 +39,7 @@ public:
static inline float get_correction() { return float(ui8_to_percent(correction)) / 100.0f; }
#else
static constexpr uint8_t correction = (BACKLASH_CORRECTION) * 0xFF;
static const float distance_mm[XYZ];
static const xyz_float_t distance_mm;
#ifdef BACKLASH_SMOOTHING_MM
static constexpr float smoothing_mm = BACKLASH_SMOOTHING_MM;
#endif
@ -47,8 +47,8 @@ public:
#if ENABLED(MEASURE_BACKLASH_WHEN_PROBING)
private:
static float measured_mm[XYZ];
static uint8_t measured_count[XYZ];
static xyz_float_t measured_mm;
static xyz_uint8_t measured_count;
public:
static void measure_with_probe();
#endif

162
Marlin/src/feature/bedlevel/abl/abl.cpp
View File

@ -35,9 +35,9 @@
#include "../../../lcd/extensible_ui/ui_api.h"
#endif
int bilinear_grid_spacing[2], bilinear_start[2];
float bilinear_grid_factor[2],
z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
xy_int_t bilinear_grid_spacing, bilinear_start;
xy_float_t bilinear_grid_factor;
bed_mesh_t z_values;
/**
* Extrapolate a single point from its neighbors
@ -153,8 +153,8 @@ void print_bilinear_leveling_grid() {
#define ABL_TEMP_POINTS_X (GRID_MAX_POINTS_X + 2)
#define ABL_TEMP_POINTS_Y (GRID_MAX_POINTS_Y + 2)
float z_values_virt[ABL_GRID_POINTS_VIRT_X][ABL_GRID_POINTS_VIRT_Y];
int bilinear_grid_spacing_virt[2] = { 0 };
float bilinear_grid_factor_virt[2] = { 0 };
xy_int_t bilinear_grid_spacing_virt;
xy_float_t bilinear_grid_factor_virt;
void print_bilinear_leveling_grid_virt() {
SERIAL_ECHOLNPGM("Subdivided with CATMULL ROM Leveling Grid:");
@ -207,7 +207,7 @@ void print_bilinear_leveling_grid() {
+ p[i] * (2 - 5 * sq(t) + 3 * t * sq(t))
+ p[i+1] * t * (1 + 4 * t - 3 * sq(t))
- p[i+2] * sq(t) * (1 - t)
) * 0.5;
) * 0.5f;
}
static float bed_level_virt_2cmr(const uint8_t x, const uint8_t y, const float &tx, const float &ty) {
@ -222,10 +222,8 @@ void print_bilinear_leveling_grid() {
}
void bed_level_virt_interpolate() {
bilinear_grid_spacing_virt[X_AXIS] = bilinear_grid_spacing[X_AXIS] / (BILINEAR_SUBDIVISIONS);
bilinear_grid_spacing_virt[Y_AXIS] = bilinear_grid_spacing[Y_AXIS] / (BILINEAR_SUBDIVISIONS);
bilinear_grid_factor_virt[X_AXIS] = RECIPROCAL(bilinear_grid_spacing_virt[X_AXIS]);
bilinear_grid_factor_virt[Y_AXIS] = RECIPROCAL(bilinear_grid_spacing_virt[Y_AXIS]);
bilinear_grid_spacing_virt = bilinear_grid_spacing / (BILINEAR_SUBDIVISIONS);
bilinear_grid_factor_virt = bilinear_grid_spacing_virt.reciprocal();
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
for (uint8_t ty = 0; ty < BILINEAR_SUBDIVISIONS; ty++)
@ -245,40 +243,38 @@ void print_bilinear_leveling_grid() {
// Refresh after other values have been updated
void refresh_bed_level() {
bilinear_grid_factor[X_AXIS] = RECIPROCAL(bilinear_grid_spacing[X_AXIS]);
bilinear_grid_factor[Y_AXIS] = RECIPROCAL(bilinear_grid_spacing[Y_AXIS]);
bilinear_grid_factor = bilinear_grid_spacing.reciprocal();
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
bed_level_virt_interpolate();
#endif
}
#if ENABLED(ABL_BILINEAR_SUBDIVISION)
#define ABL_BG_SPACING(A) bilinear_grid_spacing_virt[A]
#define ABL_BG_FACTOR(A) bilinear_grid_factor_virt[A]
#define ABL_BG_SPACING(A) bilinear_grid_spacing_virt.A
#define ABL_BG_FACTOR(A) bilinear_grid_factor_virt.A
#define ABL_BG_POINTS_X ABL_GRID_POINTS_VIRT_X
#define ABL_BG_POINTS_Y ABL_GRID_POINTS_VIRT_Y
#define ABL_BG_GRID(X,Y) z_values_virt[X][Y]
#else
#define ABL_BG_SPACING(A) bilinear_grid_spacing[A]
#define ABL_BG_FACTOR(A) bilinear_grid_factor[A]
#define ABL_BG_SPACING(A) bilinear_grid_spacing.A
#define ABL_BG_FACTOR(A) bilinear_grid_factor.A
#define ABL_BG_POINTS_X GRID_MAX_POINTS_X
#define ABL_BG_POINTS_Y GRID_MAX_POINTS_Y
#define ABL_BG_GRID(X,Y) z_values[X][Y]
#endif
// Get the Z adjustment for non-linear bed leveling
float bilinear_z_offset(const float raw[XYZ]) {
float bilinear_z_offset(const xy_pos_t &raw) {
static float z1, d2, z3, d4, L, D, ratio_x, ratio_y,
last_x = -999.999, last_y = -999.999;
static float z1, d2, z3, d4, L, D;
static xy_pos_t prev { -999.999, -999.999 }, ratio;
// Whole units for the grid line indices. Constrained within bounds.
static int8_t gridx, gridy, nextx, nexty,
last_gridx = -99, last_gridy = -99;
static xy_int8_t thisg, nextg, lastg { -99, -99 };
// XY relative to the probed area
const float rx = raw[X_AXIS] - bilinear_start[X_AXIS],
ry = raw[Y_AXIS] - bilinear_start[Y_AXIS];
xy_pos_t rel = raw - bilinear_start.asFloat();
#if ENABLED(EXTRAPOLATE_BEYOND_GRID)
#define FAR_EDGE_OR_BOX 2 // Keep using the last grid box
@ -286,63 +282,62 @@ float bilinear_z_offset(const float raw[XYZ]) {
#define FAR_EDGE_OR_BOX 1 // Just use the grid far edge
#endif
if (last_x != rx) {
last_x = rx;
ratio_x = rx * ABL_BG_FACTOR(X_AXIS);
const float gx = constrain(FLOOR(ratio_x), 0, ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX));
ratio_x -= gx; // Subtract whole to get the ratio within the grid box
if (prev.x != rel.x) {
prev.x = rel.x;
ratio.x = rel.x * ABL_BG_FACTOR(x);
const float gx = constrain(FLOOR(ratio.x), 0, ABL_BG_POINTS_X - (FAR_EDGE_OR_BOX));
ratio.x -= gx; // Subtract whole to get the ratio within the grid box
#if DISABLED(EXTRAPOLATE_BEYOND_GRID)
// Beyond the grid maintain height at grid edges
NOLESS(ratio_x, 0); // Never < 0.0. (> 1.0 is ok when nextx==gridx.)
NOLESS(ratio.x, 0); // Never <0 (>1 is ok when nextg.x==thisg.x)
#endif
gridx = gx;
nextx = _MIN(gridx + 1, ABL_BG_POINTS_X - 1);
thisg.x = gx;
nextg.x = _MIN(thisg.x + 1, ABL_BG_POINTS_X - 1);
}
if (last_y != ry || last_gridx != gridx) {
if (prev.y != rel.y || lastg.x != thisg.x) {
if (last_y != ry) {
last_y = ry;
ratio_y = ry * ABL_BG_FACTOR(Y_AXIS);
const float gy = constrain(FLOOR(ratio_y), 0, ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX));
ratio_y -= gy;
if (prev.y != rel.y) {
prev.y = rel.y;
ratio.y = rel.y * ABL_BG_FACTOR(y);
const float gy = constrain(FLOOR(ratio.y), 0, ABL_BG_POINTS_Y - (FAR_EDGE_OR_BOX));
ratio.y -= gy;
#if DISABLED(EXTRAPOLATE_BEYOND_GRID)
// Beyond the grid maintain height at grid edges
NOLESS(ratio_y, 0); // Never < 0.0. (> 1.0 is ok when nexty==gridy.)
NOLESS(ratio.y, 0); // Never < 0.0. (> 1.0 is ok when nextg.y==thisg.y.)
#endif
gridy = gy;
nexty = _MIN(gridy + 1, ABL_BG_POINTS_Y - 1);
thisg.y = gy;
nextg.y = _MIN(thisg.y + 1, ABL_BG_POINTS_Y - 1);
}
if (last_gridx != gridx || last_gridy != gridy) {
last_gridx = gridx;
last_gridy = gridy;
if (lastg != thisg) {
lastg = thisg;
// Z at the box corners
z1 = ABL_BG_GRID(gridx, gridy); // left-front
d2 = ABL_BG_GRID(gridx, nexty) - z1; // left-back (delta)
z3 = ABL_BG_GRID(nextx, gridy); // right-front
d4 = ABL_BG_GRID(nextx, nexty) - z3; // right-back (delta)
z1 = ABL_BG_GRID(thisg.x, thisg.y); // left-front
d2 = ABL_BG_GRID(thisg.x, nextg.y) - z1; // left-back (delta)
z3 = ABL_BG_GRID(nextg.x, thisg.y); // right-front
d4 = ABL_BG_GRID(nextg.x, nextg.y) - z3; // right-back (delta)
}
// Bilinear interpolate. Needed since ry or gridx has changed.
L = z1 + d2 * ratio_y; // Linear interp. LF -> LB
const float R = z3 + d4 * ratio_y; // Linear interp. RF -> RB
// Bilinear interpolate. Needed since rel.y or thisg.x has changed.
L = z1 + d2 * ratio.y; // Linear interp. LF -> LB
const float R = z3 + d4 * ratio.y; // Linear interp. RF -> RB
D = R - L;
}
const float offset = L + ratio_x * D; // the offset almost always changes
const float offset = L + ratio.x * D; // the offset almost always changes
/*
static float last_offset = 0;
if (ABS(last_offset - offset) > 0.2) {
SERIAL_ECHOLNPAIR("Sudden Shift at x=", rx, " / ", bilinear_grid_spacing[X_AXIS], " -> gridx=", gridx);
SERIAL_ECHOLNPAIR(" y=", ry, " / ", bilinear_grid_spacing[Y_AXIS], " -> gridy=", gridy);
SERIAL_ECHOLNPAIR(" ratio_x=", ratio_x, " ratio_y=", ratio_y);
SERIAL_ECHOLNPAIR("Sudden Shift at x=", rel.x, " / ", bilinear_grid_spacing.x, " -> thisg.x=", thisg.x);
SERIAL_ECHOLNPAIR(" y=", rel.y, " / ", bilinear_grid_spacing.y, " -> thisg.y=", thisg.y);
SERIAL_ECHOLNPAIR(" ratio.x=", ratio.x, " ratio.y=", ratio.y);
SERIAL_ECHOLNPAIR(" z1=", z1, " z2=", z2, " z3=", z3, " z4=", z4);
SERIAL_ECHOLNPAIR(" L=", L, " R=", R, " offset=", offset);
}
@ -354,7 +349,7 @@ float bilinear_z_offset(const float raw[XYZ]) {
#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)
#define CELL_INDEX(A,V) ((V - bilinear_start[_AXIS(A)]) * ABL_BG_FACTOR(_AXIS(A)))
#define CELL_INDEX(A,V) ((V - bilinear_start.A) * ABL_BG_FACTOR(A))
/**
* Prepare a bilinear-leveled linear move on Cartesian,
@ -362,62 +357,61 @@ float bilinear_z_offset(const float raw[XYZ]) {
*/
void bilinear_line_to_destination(const feedRate_t scaled_fr_mm_s, uint16_t x_splits, uint16_t y_splits) {
// 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]),
cy2 = CELL_INDEX(Y, destination[Y_AXIS]);
LIMIT(cx1, 0, ABL_BG_POINTS_X - 2);
LIMIT(cy1, 0, ABL_BG_POINTS_Y - 2);
LIMIT(cx2, 0, ABL_BG_POINTS_X - 2);
LIMIT(cy2, 0, ABL_BG_POINTS_Y - 2);
xy_int_t c1 { CELL_INDEX(x, current_position.x), CELL_INDEX(y, current_position.y) },
c2 { CELL_INDEX(x, destination.x), CELL_INDEX(y, destination.y) };
LIMIT(c1.x, 0, ABL_BG_POINTS_X - 2);
LIMIT(c1.y, 0, ABL_BG_POINTS_Y - 2);
LIMIT(c2.x, 0, ABL_BG_POINTS_X - 2);
LIMIT(c2.y, 0, ABL_BG_POINTS_Y - 2);
// Start and end in the same cell? No split needed.
if (cx1 == cx2 && cy1 == cy2) {
set_current_from_destination();
if (c1 == c2) {
current_position = destination;
line_to_current_position(scaled_fr_mm_s);
return;
}
#define LINE_SEGMENT_END(A) (current_position[_AXIS(A)] + (destination[_AXIS(A)] - current_position[_AXIS(A)]) * normalized_dist)
#define LINE_SEGMENT_END(A) (current_position.A + (destination.A - current_position.A) * normalized_dist)
float normalized_dist, end[XYZE];
const int8_t gcx = _MAX(cx1, cx2), gcy = _MAX(cy1, cy2);
float normalized_dist;
xyze_pos_t end;
const xy_int8_t gc { _MAX(c1.x, c2.x), _MAX(c1.y, c2.y) };
// 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)) {
if (c2.x != c1.x && TEST(x_splits, gc.x)) {
// 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, gc.x);
end = destination;
destination.x = bilinear_start.x + ABL_BG_SPACING(x) * gc.x;
normalized_dist = (destination.x - current_position.x) / (end.x - current_position.x);
destination.y = LINE_SEGMENT_END(y);
}
// Crosses on the Y and not already split on this Y?
else if (cy2 != cy1 && TEST(y_splits, gcy)) {
else if (c2.y != c1.y && TEST(y_splits, gc.y)) {
// 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, gc.y);
end = destination;
destination.y = bilinear_start.y + ABL_BG_SPACING(y) * gc.y;
normalized_dist = (destination.y - current_position.y) / (end.y - current_position.y);
destination.x = LINE_SEGMENT_END(x);
}
else {
// Must already have been split on these border(s)
// This should be a rare case.
set_current_from_destination();
current_position = destination;
line_to_current_position(scaled_fr_mm_s);
return;
}
destination[Z_AXIS] = LINE_SEGMENT_END(Z);
destination[E_AXIS] = LINE_SEGMENT_END(E);
destination.z = LINE_SEGMENT_END(z);
destination.e = LINE_SEGMENT_END(e);
// Do the split and look for more borders
bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
// Restore destination from stack
COPY(destination, end);
destination = end;
bilinear_line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
}

12
Marlin/src/feature/bedlevel/abl/abl.h
View File

@ -23,10 +23,10 @@
#include "../../../inc/MarlinConfigPre.h"
extern int bilinear_grid_spacing[2], bilinear_start[2];
extern float bilinear_grid_factor[2],
z_values[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
float bilinear_z_offset(const float raw[XYZ]);
extern xy_int_t bilinear_grid_spacing, bilinear_start;
extern xy_float_t bilinear_grid_factor;
extern bed_mesh_t z_values;
float bilinear_z_offset(const xy_pos_t &raw);
void extrapolate_unprobed_bed_level();
void print_bilinear_leveling_grid();
@ -40,6 +40,6 @@ void refresh_bed_level();
void bilinear_line_to_destination(const feedRate_t &scaled_fr_mm_s, uint16_t x_splits=0xFFFF, uint16_t y_splits=0xFFFF);
#endif
#define _GET_MESH_X(I) (bilinear_start[X_AXIS] + (I) * bilinear_grid_spacing[X_AXIS])
#define _GET_MESH_Y(J) (bilinear_start[Y_AXIS] + (J) * bilinear_grid_spacing[Y_AXIS])
#define _GET_MESH_X(I) float(bilinear_start.x + (I) * bilinear_grid_spacing.x)
#define _GET_MESH_Y(J) float(bilinear_start.y + (J) * bilinear_grid_spacing.y)
#define Z_VALUES_ARR z_values

32
Marlin/src/feature/bedlevel/bedlevel.cpp
View File

@ -51,7 +51,7 @@ bool leveling_is_valid() {
#if ENABLED(MESH_BED_LEVELING)
mbl.has_mesh()
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
!!bilinear_grid_spacing[X_AXIS]
!!bilinear_grid_spacing.x
#elif ENABLED(AUTO_BED_LEVELING_UBL)
ubl.mesh_is_valid()
#else // 3POINT, LINEAR
@ -81,13 +81,13 @@ void set_bed_leveling_enabled(const bool enable/*=true*/) {
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
// Force bilinear_z_offset to re-calculate next time
const float reset[XYZ] = { -9999.999, -9999.999, 0 };
const xyz_pos_t reset { -9999.999, -9999.999, 0 };
(void)bilinear_z_offset(reset);
#endif
if (planner.leveling_active) { // leveling from on to off
// change unleveled current_position to physical current_position without moving steppers.
planner.apply_leveling(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS]);
planner.apply_leveling(current_position);
planner.leveling_active = false; // disable only AFTER calling apply_leveling
}
else { // leveling from off to on
@ -116,9 +116,9 @@ TemporaryBedLevelingState::TemporaryBedLevelingState(const bool enable) : saved(
planner.set_z_fade_height(zfh);
if (leveling_was_active) {
const float oldpos[] = { current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] };
const xyz_pos_t oldpos = current_position;
set_bed_leveling_enabled(true);
if (do_report && memcmp(oldpos, current_position, sizeof(oldpos)))
if (do_report && oldpos != current_position)
report_current_position();
}
}
@ -137,8 +137,8 @@ void reset_bed_level() {
#if ENABLED(MESH_BED_LEVELING)
mbl.reset();
#elif ENABLED(AUTO_BED_LEVELING_BILINEAR)
bilinear_start[X_AXIS] = bilinear_start[Y_AXIS] =
bilinear_grid_spacing[X_AXIS] = bilinear_grid_spacing[Y_AXIS] = 0;
bilinear_start.reset();
bilinear_grid_spacing.reset();
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++) {
z_values[x][y] = NAN;
@ -223,25 +223,25 @@ void reset_bed_level() {
#if EITHER(MESH_BED_LEVELING, PROBE_MANUALLY)
void _manual_goto_xy(const float &rx, const float &ry) {
void _manual_goto_xy(const xy_pos_t &pos) {
#ifdef MANUAL_PROBE_START_Z
constexpr float startz = _MAX(0, MANUAL_PROBE_START_Z);
#if MANUAL_PROBE_HEIGHT > 0
do_blocking_move_to(rx, ry, MANUAL_PROBE_HEIGHT);
do_blocking_move_to_z(_MAX(0,MANUAL_PROBE_START_Z));
do_blocking_move_to(pos, MANUAL_PROBE_HEIGHT);
do_blocking_move_to_z(startz);
#else
do_blocking_move_to(rx, ry, _MAX(0,MANUAL_PROBE_START_Z));
do_blocking_move_to(pos, startz);
#endif
#elif MANUAL_PROBE_HEIGHT > 0
const float prev_z = current_position[Z_AXIS];
do_blocking_move_to(rx, ry, MANUAL_PROBE_HEIGHT);
const float prev_z = current_position.z;
do_blocking_move_to(pos, MANUAL_PROBE_HEIGHT);
do_blocking_move_to_z(prev_z);
#else
do_blocking_move_to_xy(rx, ry);
do_blocking_move_to_xy(pos);
#endif
current_position[X_AXIS] = rx;
current_position[Y_AXIS] = ry;
current_position = pos;
#if ENABLED(LCD_BED_LEVELING)
ui.wait_for_bl_move = false;

22
Marlin/src/feature/bedlevel/bedlevel.h
View File

@ -38,7 +38,7 @@ void reset_bed_level();
#endif
#if EITHER(MESH_BED_LEVELING, PROBE_MANUALLY)
void _manual_goto_xy(const float &x, const float &y);
void _manual_goto_xy(const xy_pos_t &pos);
#endif
/**
@ -57,11 +57,6 @@ class TemporaryBedLevelingState {
typedef float bed_mesh_t[GRID_MAX_POINTS_X][GRID_MAX_POINTS_Y];
typedef struct {
int8_t x_index, y_index;
float distance; // When populated, the distance from the search location
} mesh_index_pair;
#if ENABLED(AUTO_BED_LEVELING_BILINEAR)
#include "abl/abl.h"
#elif ENABLED(AUTO_BED_LEVELING_UBL)
@ -71,6 +66,7 @@ class TemporaryBedLevelingState {
#endif
#define Z_VALUES(X,Y) Z_VALUES_ARR[X][Y]
#define _GET_MESH_POS(M) { _GET_MESH_X(M.a), _GET_MESH_Y(M.b) }
#if EITHER(AUTO_BED_LEVELING_BILINEAR, MESH_BED_LEVELING)
@ -85,4 +81,18 @@ class TemporaryBedLevelingState {
#endif
struct mesh_index_pair {
xy_int8_t pos;
float distance; // When populated, the distance from the search location
void invalidate() { pos = -1; }
bool valid() const { return pos.x >= 0 && pos.y >= 0; }
#if ENABLED(AUTO_BED_LEVELING_UBL)
xy_pos_t meshpos() {
return { ubl.mesh_index_to_xpos(pos.x), ubl.mesh_index_to_ypos(pos.y) };
}
#endif
operator xy_int8_t&() { return pos; }
operator const xy_int8_t&() const { return pos; }
};
#endif

55
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.cpp
View File

@ -24,10 +24,9 @@
#if ENABLED(MESH_BED_LEVELING)
#include "mesh_bed_leveling.h"
#include "../bedlevel.h"
#include "../../../module/motion.h"
#include "../../../feature/bedlevel/bedlevel.h"
#if ENABLED(EXTENSIBLE_UI)
#include "../../../lcd/extensible_ui/ui_api.h"
@ -66,62 +65,60 @@
*/
void mesh_bed_leveling::line_to_destination(const feedRate_t &scaled_fr_mm_s, uint8_t x_splits, uint8_t y_splits) {
// 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]),
cy2 = cell_index_y(destination[Y_AXIS]);
NOMORE(cx1, GRID_MAX_POINTS_X - 2);
NOMORE(cy1, GRID_MAX_POINTS_Y - 2);
NOMORE(cx2, GRID_MAX_POINTS_X - 2);
NOMORE(cy2, GRID_MAX_POINTS_Y - 2);
xy_int8_t scel = cell_indexes(current_position), ecel = cell_indexes(destination);
NOMORE(scel.x, GRID_MAX_POINTS_X - 2);
NOMORE(scel.y, GRID_MAX_POINTS_Y - 2);
NOMORE(ecel.x, GRID_MAX_POINTS_X - 2);
NOMORE(ecel.y, GRID_MAX_POINTS_Y - 2);
// Start and end in the same cell? No split needed.
if (cx1 == cx2 && cy1 == cy2) {
if (scel == ecel) {
line_to_destination(scaled_fr_mm_s);
set_current_from_destination();
current_position = destination;
return;
}
#define MBL_SEGMENT_END(A) (current_position[_AXIS(A)] + (destination[_AXIS(A)] - current_position[_AXIS(A)]) * normalized_dist)
#define MBL_SEGMENT_END(A) (current_position.A + (destination.A - current_position.A) * normalized_dist)
float normalized_dist, end[XYZE];
const int8_t gcx = _MAX(cx1, cx2), gcy = _MAX(cy1, cy2);
float normalized_dist;
xyze_pos_t dest;
const int8_t gcx = _MAX(scel.x, ecel.x), gcy = _MAX(scel.y, ecel.y);
// 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)) {
if (ecel.x != scel.x && TEST(x_splits, gcx)) {
// Split on the X grid line
CBI(x_splits, gcx);
COPY(end, destination);
destination[X_AXIS] = 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);
dest = destination;
destination.x = index_to_xpos[gcx];
normalized_dist = (destination.x - current_position.x) / (dest.x - current_position.x);
destination.y = MBL_SEGMENT_END(y);
}
// Crosses on the Y and not already split on this Y?
else if (cy2 != cy1 && TEST(y_splits, gcy)) {
else if (ecel.y != scel.y && TEST(y_splits, gcy)) {
// Split on the Y grid line
CBI(y_splits, gcy);
COPY(end, destination);
destination[Y_AXIS] = 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);
dest = destination;
destination.y = index_to_ypos[gcy];
normalized_dist = (destination.y - current_position.y) / (dest.y - current_position.y);
destination.x = MBL_SEGMENT_END(x);
}
else {
// Must already have been split on these border(s)
// This should be a rare case.
line_to_destination(scaled_fr_mm_s);
set_current_from_destination();
current_position = destination;
return;
}
destination[Z_AXIS] = MBL_SEGMENT_END(Z);
destination[E_AXIS] = MBL_SEGMENT_END(E);
destination.z = MBL_SEGMENT_END(z);
destination.e = MBL_SEGMENT_END(e);
// Do the split and look for more borders
line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
// Restore destination from stack
COPY(destination, end);
destination = dest;
line_to_destination(scaled_fr_mm_s, x_splits, y_splits);
}

32
Marlin/src/feature/bedlevel/mbl/mesh_bed_leveling.h
View File

@ -76,21 +76,27 @@ public:
int8_t cx = (x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST);
return constrain(cx, 0, (GRID_MAX_POINTS_X) - 2);
}
static int8_t cell_index_y(const float &y) {
int8_t cy = (y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST);
return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 2);
}
static inline xy_int8_t cell_indexes(const float &x, const float &y) {
return { cell_index_x(x), cell_index_y(y) };
}
static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
static int8_t probe_index_x(const float &x) {
int8_t px = (x - (MESH_MIN_X) + 0.5f * (MESH_X_DIST)) * RECIPROCAL(MESH_X_DIST);
return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
}
static int8_t probe_index_y(const float &y) {
int8_t py = (y - (MESH_MIN_Y) + 0.5f * (MESH_Y_DIST)) * RECIPROCAL(MESH_Y_DIST);
return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
}
static inline xy_int8_t probe_indexes(const float &x, const float &y) {
return { probe_index_x(x), probe_index_y(y) };
}
static inline xy_int8_t probe_indexes(const xy_pos_t &xy) { return probe_indexes(xy.x, xy.y); }
static float calc_z0(const float &a0, const float &a1, const float &z1, const float &a2, const float &z2) {
const float delta_z = (z2 - z1) / (a2 - a1),
@ -98,21 +104,21 @@ public:
return z1 + delta_a * delta_z;
}
static float get_z(const float &x0, const float &y0
static float get_z(const xy_pos_t &pos
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
, const float &factor
, const float &factor=1.0f
#endif
) {
const int8_t cx = cell_index_x(x0), cy = cell_index_y(y0);
const float z1 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy], index_to_xpos[cx + 1], z_values[cx + 1][cy]),
z2 = calc_z0(x0, index_to_xpos[cx], z_values[cx][cy + 1], index_to_xpos[cx + 1], z_values[cx + 1][cy + 1]),
z0 = calc_z0(y0, index_to_ypos[cy], z1, index_to_ypos[cy + 1], z2);
#if DISABLED(ENABLE_LEVELING_FADE_HEIGHT)
constexpr float factor = 1.0f;
#endif
const xy_int8_t ind = cell_indexes(pos);
const float x1 = index_to_xpos[ind.x], x2 = index_to_xpos[ind.x+1],
y1 = index_to_xpos[ind.y], y2 = index_to_xpos[ind.y+1],
z1 = calc_z0(pos.x, x1, z_values[ind.x][ind.y ], x2, z_values[ind.x+1][ind.y ]),
z2 = calc_z0(pos.x, x1, z_values[ind.x][ind.y+1], x2, z_values[ind.x+1][ind.y+1]);
return z_offset + z0
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
* factor
#endif
;
return z_offset + calc_z0(pos.y, y1, z1, y2, z2) * factor;
}
#if IS_CARTESIAN && DISABLED(SEGMENT_LEVELED_MOVES)

5
Marlin/src/feature/bedlevel/ubl/ubl.cpp
View File

@ -176,8 +176,7 @@
// Add XY probe offset from extruder because probe_at_point() subtracts them when
// moving to the XY position to be measured. This ensures better agreement between
// the current Z position after G28 and the mesh values.
const float current_xi = find_closest_x_index(current_position[X_AXIS] + probe_offset[X_AXIS]),
current_yi = find_closest_y_index(current_position[Y_AXIS] + probe_offset[Y_AXIS]);
const xy_int8_t curr = closest_indexes(xy_pos_t(current_position) + xy_pos_t(probe_offset));
if (!lcd) SERIAL_EOL();
for (int8_t j = GRID_MAX_POINTS_Y - 1; j >= 0; j--) {
@ -193,7 +192,7 @@
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
// Opening Brace or Space
const bool is_current = i == current_xi && j == current_yi;
const bool is_current = i == curr.x && j == curr.y;
if (human) SERIAL_CHAR(is_current ? '[' : ' ');
// Z Value at current I, J

47
Marlin/src/feature/bedlevel/ubl/ubl.h
View File

@ -32,15 +32,12 @@
#define UBL_OK false
#define UBL_ERR true
#define USE_NOZZLE_AS_REFERENCE 0
#define USE_PROBE_AS_REFERENCE 1
// ubl_G29.cpp
enum MeshPointType : char { INVALID, REAL, SET_IN_BITMAP };
// External references
struct mesh_index_pair;
#define MESH_X_DIST (float(MESH_MAX_X - (MESH_MIN_X)) / float(GRID_MAX_POINTS_X - 1))
#define MESH_Y_DIST (float(MESH_MAX_Y - (MESH_MIN_Y)) / float(GRID_MAX_POINTS_Y - 1))
@ -52,10 +49,11 @@ class unified_bed_leveling {
g29_repetition_cnt,
g29_storage_slot,
g29_map_type;
static bool g29_c_flag, g29_x_flag, g29_y_flag;
static float g29_x_pos, g29_y_pos,
g29_card_thickness,
static bool g29_c_flag;
static float g29_card_thickness,
g29_constant;
static xy_pos_t g29_pos;
static xy_bool_t xy_seen;
#if HAS_BED_PROBE
static int g29_grid_size;
@ -65,16 +63,19 @@ class unified_bed_leveling {
static void move_z_with_encoder(const float &multiplier);
static float measure_point_with_encoder();
static float measure_business_card_thickness(float in_height);
static void manually_probe_remaining_mesh(const float&, const float&, const float&, const float&, const bool) _O0;
static void fine_tune_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map) _O0;
static void manually_probe_remaining_mesh(const xy_pos_t&, const float&, const float&, const bool) _O0;
static void fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) _O0;
#endif
static bool g29_parameter_parsing() _O0;
static void shift_mesh_height();
static void probe_entire_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) _O0;
static void probe_entire_mesh(const xy_pos_t &near, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) _O0;
static void tilt_mesh_based_on_3pts(const float &z1, const float &z2, const float &z3);
static void tilt_mesh_based_on_probed_grid(const bool do_ubl_mesh_map);
static bool smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir);
static inline bool smart_fill_one(const xy_uint8_t &pos, const xy_uint8_t &dir) {
return smart_fill_one(pos.x, pos.y, dir.x, dir.y);
}
static void smart_fill_mesh();
#if ENABLED(UBL_DEVEL_DEBUGGING)
@ -91,7 +92,7 @@ class unified_bed_leveling {
static void save_ubl_active_state_and_disable();
static void restore_ubl_active_state_and_leave();
static void display_map(const int) _O0;
static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const float&, const float&, const bool, uint16_t[16]) _O0;
static mesh_index_pair find_closest_mesh_point_of_type(const MeshPointType, const xy_pos_t&, const bool=false, MeshFlags *done_flags=nullptr) _O0;
static mesh_index_pair find_furthest_invalid_mesh_point() _O0;
static void reset();
static void invalidate();
@ -118,14 +119,14 @@ class unified_bed_leveling {
FORCE_INLINE static void set_z(const int8_t px, const int8_t py, const float &z) { z_values[px][py] = z; }
static int8_t get_cell_index_x(const float &x) {
static int8_t cell_index_x(const float &x) {
const int8_t cx = (x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST);
return constrain(cx, 0, (GRID_MAX_POINTS_X) - 1); // -1 is appropriate if we want all movement to the X_MAX
} // position. But with this defined this way, it is possible
// to extrapolate off of this point even further out. Probably
// that is OK because something else should be keeping that from
// happening and should not be worried about at this level.
static int8_t get_cell_index_y(const float &y) {
static int8_t cell_index_y(const float &y) {
const int8_t cy = (y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST);
return constrain(cy, 0, (GRID_MAX_POINTS_Y) - 1); // -1 is appropriate if we want all movement to the Y_MAX
} // position. But with this defined this way, it is possible
@ -133,15 +134,22 @@ class unified_bed_leveling {
// that is OK because something else should be keeping that from
// happening and should not be worried about at this level.
static int8_t find_closest_x_index(const float &x) {
static inline xy_int8_t cell_indexes(const float &x, const float &y) {
return { cell_index_x(x), cell_index_y(y) };
}
static inline xy_int8_t cell_indexes(const xy_pos_t &xy) { return cell_indexes(xy.x, xy.y); }
static int8_t closest_x_index(const float &x) {
const int8_t px = (x - (MESH_MIN_X) + (MESH_X_DIST) * 0.5) * RECIPROCAL(MESH_X_DIST);
return WITHIN(px, 0, GRID_MAX_POINTS_X - 1) ? px : -1;
}
static int8_t find_closest_y_index(const float &y) {
static int8_t closest_y_index(const float &y) {
const int8_t py = (y - (MESH_MIN_Y) + (MESH_Y_DIST) * 0.5) * RECIPROCAL(MESH_Y_DIST);
return WITHIN(py, 0, GRID_MAX_POINTS_Y - 1) ? py : -1;
}
static inline xy_int8_t closest_indexes(const xy_pos_t &xy) {
return { closest_x_index(xy.x), closest_y_index(xy.y) };
}
/**
* z2 --|
@ -228,8 +236,7 @@ class unified_bed_leveling {
* on the Y position within the cell.
*/
static float get_z_correction(const float &rx0, const float &ry0) {
const int8_t cx = get_cell_index_x(rx0),
cy = get_cell_index_y(ry0); // return values are clamped
const int8_t cx = cell_index_x(rx0), cy = cell_index_y(ry0); // return values are clamped
/**
* Check if the requested location is off the mesh. If so, and
@ -275,11 +282,11 @@ class unified_bed_leveling {
}
return z0;
}
static inline float get_z_correction(const xy_pos_t &pos) { return get_z_correction(pos.x, pos.y); }
static inline float mesh_index_to_xpos(const uint8_t i) {
return i < GRID_MAX_POINTS_X ? pgm_read_float(&_mesh_index_to_xpos[i]) : MESH_MIN_X + i * (MESH_X_DIST);
}
static inline float mesh_index_to_ypos(const uint8_t i) {
return i < GRID_MAX_POINTS_Y ? pgm_read_float(&_mesh_index_to_ypos[i]) : MESH_MIN_Y + i * (MESH_Y_DIST);
}

490
Marlin/src/feature/bedlevel/ubl/ubl_G29.cpp
View File

@ -53,8 +53,6 @@
#define UBL_G29_P31
extern float destination[XYZE], current_position[XYZE];
#if HAS_LCD_MENU
void _lcd_ubl_output_map_lcd();
#endif
@ -67,13 +65,11 @@
unified_bed_leveling::g29_repetition_cnt,
unified_bed_leveling::g29_storage_slot = 0,
unified_bed_leveling::g29_map_type;
bool unified_bed_leveling::g29_c_flag,
unified_bed_leveling::g29_x_flag,
unified_bed_leveling::g29_y_flag;
float unified_bed_leveling::g29_x_pos,
unified_bed_leveling::g29_y_pos,
unified_bed_leveling::g29_card_thickness = 0,
bool unified_bed_leveling::g29_c_flag;
float unified_bed_leveling::g29_card_thickness = 0,
unified_bed_leveling::g29_constant = 0;
xy_bool_t unified_bed_leveling::xy_seen;
xy_pos_t unified_bed_leveling::g29_pos;
#if HAS_BED_PROBE
int unified_bed_leveling::g29_grid_size;
@ -330,18 +326,19 @@
else {
while (g29_repetition_cnt--) {
if (cnt > 20) { cnt = 0; idle(); }
const mesh_index_pair location = find_closest_mesh_point_of_type(REAL, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, nullptr);
if (location.x_index < 0) {
// No more REACHABLE mesh points to invalidate, so we ASSUME the user
const mesh_index_pair closest = find_closest_mesh_point_of_type(REAL, g29_pos);
const xy_int8_t &cpos = closest.pos;
if (cpos.x < 0) {
// No more REAL mesh points to invalidate, so we ASSUME the user
// meant to invalidate the ENTIRE mesh, which cannot be done with
// find_closest_mesh_point loop which only returns REACHABLE points.
// find_closest_mesh_point loop which only returns REAL points.
set_all_mesh_points_to_value(NAN);
SERIAL_ECHOLNPGM("Entire Mesh invalidated.\n");
break; // No more invalid Mesh Points to populate
}
z_values[location.x_index][location.y_index] = NAN;
z_values[cpos.x][cpos.y] = NAN;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(location.x_index, location.y_index, 0);
ExtUI::onMeshUpdate(closest, 0);
#endif
cnt++;
}
@ -448,13 +445,13 @@
SERIAL_ECHOLNPGM("Mesh invalidated. Probing mesh.");
}
if (g29_verbose_level > 1) {
SERIAL_ECHOPAIR("Probing around (", g29_x_pos);
SERIAL_ECHOPAIR("Probing around (", g29_pos.x);
SERIAL_CHAR(',');
SERIAL_ECHO(g29_y_pos);
SERIAL_ECHO(g29_pos.y);
SERIAL_ECHOLNPGM(").\n");
}
probe_entire_mesh(g29_x_pos + probe_offset[X_AXIS], g29_y_pos + probe_offset[Y_AXIS],
parser.seen('T'), parser.seen('E'), parser.seen('U'));
const xy_pos_t near = g29_pos + probe_offset;
probe_entire_mesh(near, parser.seen('T'), parser.seen('E'), parser.seen('U'));
report_current_position();
probe_deployed = true;
@ -470,7 +467,7 @@
SERIAL_ECHOLNPGM("Manually probing unreachable mesh locations.");
do_blocking_move_to_z(Z_CLEARANCE_BETWEEN_PROBES);
if (parser.seen('C') && !g29_x_flag && !g29_y_flag) {
if (parser.seen('C') && !xy_seen) {
/**
* Use a good default location for the path.
* The flipped > and < operators in these comparisons is intentional.
@ -478,13 +475,14 @@
* It may make sense to have Delta printers default to the center of the bed.
* Until that is decided, this can be forced with the X and Y parameters.
*/
#if IS_KINEMATIC
g29_x_pos = X_HOME_POS;
g29_y_pos = Y_HOME_POS;
#else // cartesian
g29_x_pos = probe_offset[X_AXIS] > 0 ? X_BED_SIZE : 0;
g29_y_pos = probe_offset[Y_AXIS] < 0 ? Y_BED_SIZE : 0;
#endif
g29_pos.set(
#if IS_KINEMATIC
X_HOME_POS, Y_HOME_POS
#else
probe_offset.x > 0 ? X_BED_SIZE : 0,
probe_offset.y < 0 ? Y_BED_SIZE : 0
#endif
);
}
if (parser.seen('B')) {
@ -496,13 +494,13 @@
probe_deployed = true;
}
if (!position_is_reachable(g29_x_pos, g29_y_pos)) {
if (!position_is_reachable(g29_pos)) {
SERIAL_ECHOLNPGM("XY outside printable radius.");
return;
}
const float height = parser.floatval('H', Z_CLEARANCE_BETWEEN_PROBES);
manually_probe_remaining_mesh(g29_x_pos, g29_y_pos, height, g29_card_thickness, parser.seen('T'));
manually_probe_remaining_mesh(g29_pos, height, g29_card_thickness, parser.seen('T'));
SERIAL_ECHOLNPGM("G29 P2 finished.");
@ -530,20 +528,22 @@
}
else {
while (g29_repetition_cnt--) { // this only populates reachable mesh points near
const mesh_index_pair location = find_closest_mesh_point_of_type(INVALID, g29_x_pos, g29_y_pos, USE_NOZZLE_AS_REFERENCE, nullptr);
if (location.x_index < 0) {
// No more REACHABLE INVALID mesh points to populate, so we ASSUME
const mesh_index_pair closest = find_closest_mesh_point_of_type(INVALID, g29_pos);
const xy_int8_t &cpos = closest.pos;
if (cpos.x < 0) {
// No more REAL INVALID mesh points to populate, so we ASSUME
// user meant to populate ALL INVALID mesh points to value
for (uint8_t x = 0; x < GRID_MAX_POINTS_X; x++)
for (uint8_t y = 0; y < GRID_MAX_POINTS_Y; y++)
if (isnan(z_values[x][y]))
z_values[x][y] = g29_constant;
if (isnan(z_values[x][y])) z_values[x][y] = g29_constant;
break; // No more invalid Mesh Points to populate
}
z_values[location.x_index][location.y_index] = g29_constant;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(location.x_index, location.y_index, z_values[location.x_index][location.y_index]);
#endif
else {
z_values[cpos.x][cpos.y] = g29_constant;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(closest, g29_constant);
#endif
}
}
}
}
@ -576,7 +576,7 @@
case 4: // Fine Tune (i.e., Edit) the Mesh
#if HAS_LCD_MENU
fine_tune_mesh(g29_x_pos, g29_y_pos, parser.seen('T'));
fine_tune_mesh(g29_pos, parser.seen('T'));
#else
SERIAL_ECHOLNPGM("?P4 is only available when an LCD is present.");
return;
@ -740,9 +740,7 @@
* Probe all invalidated locations of the mesh that can be reached by the probe.
* This attempts to fill in locations closest to the nozzle's start location first.
*/
void unified_bed_leveling::probe_entire_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) {
mesh_index_pair location;
void unified_bed_leveling::probe_entire_mesh(const xy_pos_t &near, const bool do_ubl_mesh_map, const bool stow_probe, const bool do_furthest) {
#if HAS_LCD_MENU
ui.capture();
#endif
@ -752,6 +750,7 @@
uint8_t count = GRID_MAX_POINTS;
mesh_index_pair best;
do {
if (do_ubl_mesh_map) display_map(g29_map_type);
@ -773,23 +772,23 @@
}
#endif
if (do_furthest)
location = find_furthest_invalid_mesh_point();
else
location = find_closest_mesh_point_of_type(INVALID, rx, ry, USE_PROBE_AS_REFERENCE, nullptr);
best = do_furthest
? find_furthest_invalid_mesh_point()
: find_closest_mesh_point_of_type(INVALID, near, true);
if (location.x_index >= 0) { // mesh point found and is reachable by probe
const float rawx = mesh_index_to_xpos(location.x_index),
rawy = mesh_index_to_ypos(location.y_index),
measured_z = probe_at_point(rawx, rawy, stow_probe ? PROBE_PT_STOW : PROBE_PT_RAISE, g29_verbose_level); // TODO: Needs error handling
z_values[location.x_index][location.y_index] = measured_z;
if (best.pos.x >= 0) { // mesh point found and is reachable by probe
const float measured_z = probe_at_point(
best.meshpos(),
stow_probe ? PROBE_PT_STOW : PROBE_PT_RAISE, g29_verbose_level
);
z_values[best.pos.x][best.pos.y] = measured_z;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(location.x_index, location.y_index, measured_z);
ExtUI::onMeshUpdate(best, measured_z);
#endif
}
SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
} while (location.x_index >= 0 && --count);
} while (best.pos.x >= 0 && --count);
STOW_PROBE();
@ -800,8 +799,8 @@
restore_ubl_active_state_and_leave();
do_blocking_move_to_xy(
constrain(rx - probe_offset[X_AXIS], MESH_MIN_X, MESH_MAX_X),
constrain(ry - probe_offset[Y_AXIS], MESH_MIN_Y, MESH_MAX_Y)
constrain(near.x - probe_offset.x, MESH_MIN_X, MESH_MAX_X),
constrain(near.y - probe_offset.y, MESH_MIN_Y, MESH_MAX_Y)
);
}
@ -835,7 +834,7 @@
idle();
gcode.reset_stepper_timeout(); // Keep steppers powered
if (encoder_diff) {
do_blocking_move_to_z(current_position[Z_AXIS] + float(encoder_diff) * multiplier);
do_blocking_move_to_z(current_position.z + float(encoder_diff) * multiplier);
encoder_diff = 0;
}
}
@ -844,7 +843,7 @@
float unified_bed_leveling::measure_point_with_encoder() {
KEEPALIVE_STATE(PAUSED_FOR_USER);
move_z_with_encoder(0.01f);
return current_position[Z_AXIS];
return current_position.z;
}
static void echo_and_take_a_measurement() { SERIAL_ECHOLNPGM(" and take a measurement."); }
@ -863,7 +862,7 @@
echo_and_take_a_measurement();
const float z1 = measure_point_with_encoder();
do_blocking_move_to_z(current_position[Z_AXIS] + SIZE_OF_LITTLE_RAISE);
do_blocking_move_to_z(current_position.z + SIZE_OF_LITTLE_RAISE);
planner.synchronize();
SERIAL_ECHOPGM("Remove shim");
@ -872,7 +871,7 @@
const float z2 = measure_point_with_encoder();
do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_BETWEEN_PROBES);
do_blocking_move_to_z(current_position.z + Z_CLEARANCE_BETWEEN_PROBES);
const float thickness = ABS(z1 - z2);
@ -888,29 +887,33 @@
return thickness;
}
void unified_bed_leveling::manually_probe_remaining_mesh(const float &rx, const float &ry, const float &z_clearance, const float &thick, const bool do_ubl_mesh_map) {
void unified_bed_leveling::manually_probe_remaining_mesh(const xy_pos_t &pos, const float &z_clearance, const float &thick, const bool do_ubl_mesh_map) {
ui.capture();
save_ubl_active_state_and_disable(); // No bed level correction so only raw data is obtained
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_clearance);
do_blocking_move_to(current_position.x, current_position.y, z_clearance);
ui.return_to_status();
mesh_index_pair location;
xy_int8_t &lpos = location.pos;
do {
location = find_closest_mesh_point_of_type(INVALID, rx, ry, USE_NOZZLE_AS_REFERENCE, nullptr);
location = find_closest_mesh_point_of_type(INVALID, pos);
// It doesn't matter if the probe can't reach the NAN location. This is a manual probe.
if (location.x_index < 0 && location.y_index < 0) continue;
if (!location.valid()) continue;
const float xProbe = mesh_index_to_xpos(location.x_index),
yProbe = mesh_index_to_ypos(location.y_index);
const xyz_pos_t ppos = {
mesh_index_to_xpos(lpos.x),
mesh_index_to_ypos(lpos.y),
Z_CLEARANCE_BETWEEN_PROBES
};
if (!position_is_reachable(xProbe, yProbe)) break; // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points)
if (!position_is_reachable(ppos)) break; // SHOULD NOT OCCUR (find_closest_mesh_point only returns reachable points)
LCD_MESSAGEPGM(MSG_UBL_MOVING_TO_NEXT);
do_blocking_move_to(xProbe, yProbe, Z_CLEARANCE_BETWEEN_PROBES);
do_blocking_move_to(ppos);
do_blocking_move_to_z(z_clearance);
KEEPALIVE_STATE(PAUSED_FOR_USER);
@ -932,20 +935,20 @@
return restore_ubl_active_state_and_leave();
}
z_values[location.x_index][location.y_index] = current_position[Z_AXIS] - thick;
z_values[lpos.x][lpos.y] = current_position.z - thick;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(location.x_index, location.y_index, z_values[location.x_index][location.y_index]);
ExtUI::onMeshUpdate(location, z_values[lpos.x][lpos.y]);
#endif
if (g29_verbose_level > 2)
SERIAL_ECHOLNPAIR_F("Mesh Point Measured at: ", z_values[location.x_index][location.y_index], 6);
SERIAL_ECHOLNPAIR_F("Mesh Point Measured at: ", z_values[lpos.x][lpos.y], 6);
SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
} while (location.x_index >= 0 && location.y_index >= 0);
} while (location.valid());
if (do_ubl_mesh_map) display_map(g29_map_type); // show user where we're probing
restore_ubl_active_state_and_leave();
do_blocking_move_to(rx, ry, Z_CLEARANCE_DEPLOY_PROBE);
do_blocking_move_to(pos, Z_CLEARANCE_DEPLOY_PROBE);
}
inline void set_message_with_feedback(PGM_P const msg_P) {
@ -959,8 +962,8 @@
set_message_with_feedback(PSTR(MSG_EDITING_STOPPED));
}
void unified_bed_leveling::fine_tune_mesh(const float &rx, const float &ry, const bool do_ubl_mesh_map) {
if (!parser.seen('R')) // fine_tune_mesh() is special. If no repetition count flag is specified
void unified_bed_leveling::fine_tune_mesh(const xy_pos_t &pos, const bool do_ubl_mesh_map) {
if (!parser.seen('R')) // fine_tune_mesh() is special. If no repetition count flag is specified
g29_repetition_cnt = 1; // do exactly one mesh location. Otherwise use what the parser decided.
#if ENABLED(UBL_MESH_EDIT_MOVES_Z)
@ -973,7 +976,7 @@
mesh_index_pair location;
if (!position_is_reachable(rx, ry)) {
if (!position_is_reachable(pos)) {
SERIAL_ECHOLNPGM("(X,Y) outside printable radius.");
return;
}
@ -981,76 +984,78 @@
save_ubl_active_state_and_disable();
LCD_MESSAGEPGM(MSG_UBL_FINE_TUNE_MESH);
ui.capture(); // Take over control of the LCD encoder
ui.capture(); // Take over control of the LCD encoder
do_blocking_move_to(rx, ry, Z_CLEARANCE_BETWEEN_PROBES); // Move to the given XY with probe clearance
do_blocking_move_to(pos, Z_CLEARANCE_BETWEEN_PROBES); // Move to the given XY with probe clearance
#if ENABLED(UBL_MESH_EDIT_MOVES_Z)
do_blocking_move_to_z(h_offset); // Move Z to the given 'H' offset
do_blocking_move_to_z(h_offset); // Move Z to the given 'H' offset
#endif
uint16_t not_done[16];
memset(not_done, 0xFF, sizeof(not_done));
MeshFlags done_flags{0};
xy_int8_t &lpos = location.pos;
do {
location = find_closest_mesh_point_of_type(SET_IN_BITMAP, rx, ry, USE_NOZZLE_AS_REFERENCE, not_done);
location = find_closest_mesh_point_of_type(SET_IN_BITMAP, pos, false, &done_flags);
if (location.x_index < 0) break; // Stop when there are no more reachable points
if (lpos.x < 0) break; // Stop when there are no more reachable points
bitmap_clear(not_done, location.x_index, location.y_index); // Mark this location as 'adjusted' so a new
// location is used on the next loop
done_flags.mark(lpos); // Mark this location as 'adjusted' so a new
// location is used on the next loop
const xyz_pos_t raw = {
mesh_index_to_xpos(lpos.x),
mesh_index_to_ypos(lpos.y),
Z_CLEARANCE_BETWEEN_PROBES
};
const float rawx = mesh_index_to_xpos(location.x_index),
rawy = mesh_index_to_ypos(location.y_index);
if (!position_is_reachable(raw)) break; // SHOULD NOT OCCUR (find_closest_mesh_point_of_type only returns reachable)
if (!position_is_reachable(rawx, rawy)) break; // SHOULD NOT OCCUR because find_closest_mesh_point_of_type will only return reachable
do_blocking_move_to(rawx, rawy, Z_CLEARANCE_BETWEEN_PROBES); // Move the nozzle to the edit point with probe clearance
do_blocking_move_to(raw); // Move the nozzle to the edit point with probe clearance
#if ENABLED(UBL_MESH_EDIT_MOVES_Z)
do_blocking_move_to_z(h_offset); // Move Z to the given 'H' offset before editing
do_blocking_move_to_z(h_offset); // Move Z to the given 'H' offset before editing
#endif
KEEPALIVE_STATE(PAUSED_FOR_USER);
if (do_ubl_mesh_map) display_map(g29_map_type); // Display the current point
if (do_ubl_mesh_map) display_map(g29_map_type); // Display the current point
ui.refresh();
float new_z = z_values[location.x_index][location.y_index];
if (isnan(new_z)) new_z = 0; // Invalid points begin at 0
new_z = FLOOR(new_z * 1000) * 0.001f; // Chop off digits after the 1000ths place
float new_z = z_values[lpos.x][lpos.y];
if (isnan(new_z)) new_z = 0; // Invalid points begin at 0
new_z = FLOOR(new_z * 1000) * 0.001f; // Chop off digits after the 1000ths place
lcd_mesh_edit_setup(new_z);
do {
new_z = lcd_mesh_edit();
#if ENABLED(UBL_MESH_EDIT_MOVES_Z)
do_blocking_move_to_z(h_offset + new_z); // Move the nozzle as the point is edited
do_blocking_move_to_z(h_offset + new_z); // Move the nozzle as the point is edited
#endif
idle();
SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
SERIAL_FLUSH(); // Prevent host M105 buffer overrun.
} while (!ui.button_pressed());
if (!lcd_map_control) ui.return_to_status(); // Just editing a single point? Return to status
if (!lcd_map_control) ui.return_to_status(); // Just editing a single point? Return to status
if (click_and_hold(abort_fine_tune)) break; // Button held down? Abort editing
if (click_and_hold(abort_fine_tune)) break; // Button held down? Abort editing
z_values[location.x_index][location.y_index] = new_z; // Save the updated Z value
z_values[lpos.x][lpos.y] = new_z; // Save the updated Z value
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(location.x_index, location.y_index, new_z);
ExtUI::onMeshUpdate(location, new_z);
#endif
serial_delay(20); // No switch noise
serial_delay(20); // No switch noise
ui.refresh();
} while (location.x_index >= 0 && --g29_repetition_cnt > 0);
} while (lpos.x >= 0 && --g29_repetition_cnt > 0);
ui.release();
if (do_ubl_mesh_map) display_map(g29_map_type);
restore_ubl_active_state_and_leave();
do_blocking_move_to(rx, ry, Z_CLEARANCE_BETWEEN_PROBES);
do_blocking_move_to(pos, Z_CLEARANCE_BETWEEN_PROBES);
LCD_MESSAGEPGM(MSG_UBL_DONE_EDITING_MESH);
SERIAL_ECHOLNPGM("Done Editing Mesh");
@ -1073,11 +1078,6 @@
g29_constant = 0;
g29_repetition_cnt = 0;
g29_x_flag = parser.seenval('X');
g29_x_pos = g29_x_flag ? parser.value_float() : current_position[X_AXIS];
g29_y_flag = parser.seenval('Y');
g29_y_pos = g29_y_flag ? parser.value_float() : current_position[Y_AXIS];
if (parser.seen('R')) {
g29_repetition_cnt = parser.has_value() ? parser.value_int() : GRID_MAX_POINTS;
NOMORE(g29_repetition_cnt, GRID_MAX_POINTS);
@ -1124,17 +1124,24 @@
#endif
}
if (g29_x_flag != g29_y_flag) {
xy_seen.x = parser.seenval('X');
float sx = xy_seen.x ? parser.value_float() : current_position.x;
xy_seen.y = parser.seenval('Y');
float sy = xy_seen.y ? parser.value_float() : current_position.y;
if (xy_seen.x != xy_seen.y) {
SERIAL_ECHOLNPGM("Both X & Y locations must be specified.\n");
err_flag = true;
}
// If X or Y are not valid, use center of the bed values
if (!WITHIN(g29_x_pos, X_MIN_BED, X_MAX_BED)) g29_x_pos = X_CENTER;
if (!WITHIN(g29_y_pos, Y_MIN_BED, Y_MAX_BED)) g29_y_pos = Y_CENTER;
if (!WITHIN(sx, X_MIN_BED, X_MAX_BED)) sx = X_CENTER;
if (!WITHIN(sy, Y_MIN_BED, Y_MAX_BED)) sy = Y_CENTER;
if (err_flag) return UBL_ERR;
g29_pos.set(sx, sy);
/**
* Activate or deactivate UBL
* Note: UBL's G29 restores the state set here when done.
@ -1213,26 +1220,22 @@
mesh_index_pair unified_bed_leveling::find_furthest_invalid_mesh_point() {
bool found_a_NAN = false, found_a_real = false;
bool found_a_NAN = false, found_a_real = false;
mesh_index_pair out_mesh;
out_mesh.x_index = out_mesh.y_index = -1;
out_mesh.distance = -99999.99f;
mesh_index_pair farthest { -1, -1, -99999.99 };
for (int8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (int8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
if (isnan(z_values[i][j])) { // Check to see if this location holds an invalid mesh point
if (isnan(z_values[i][j])) { // Invalid mesh point?
const float mx = mesh_index_to_xpos(i),
my = mesh_index_to_ypos(j);
if (!position_is_reachable_by_probe(mx, my)) // make sure the probe can get to the mesh point
// Skip points the probe can't reach
if (!position_is_reachable_by_probe(mesh_index_to_xpos(i), mesh_index_to_ypos(j)))
continue;
found_a_NAN = true;
int8_t closest_x = -1, closest_y = -1;
xy_int8_t near { -1, -1 };
float d1, d2 = 99999.9f;
for (int8_t k = 0; k < GRID_MAX_POINTS_X; k++) {
for (int8_t l = 0; l < GRID_MAX_POINTS_Y; l++) {
@ -1245,84 +1248,75 @@
d1 = HYPOT(i - k, j - l) + (1.0f / ((millis() % 47) + 13));
if (d1 < d2) { // found a closer distance from invalid mesh point at (i,j) to defined mesh point at (k,l)
d2 = d1; // found a closer location with
closest_x = i; // an assigned mesh point value
closest_y = j;
if (d1 < d2) { // Invalid mesh point (i,j) is closer to the defined point (k,l)
d2 = d1;
near.set(i, j);
}
}
}
}
//
// At this point d2 should have the closest defined mesh point to invalid mesh point (i,j)
// At this point d2 should have the near defined mesh point to invalid mesh point (i,j)
//
if (found_a_real && (closest_x >= 0) && (d2 > out_mesh.distance)) {
out_mesh.distance = d2; // found an invalid location with a greater distance
out_mesh.x_index = closest_x; // to a defined mesh point
out_mesh.y_index = closest_y;
if (found_a_real && near.x >= 0 && d2 > farthest.distance) {
farthest.pos = near; // Found an invalid location farther from the defined mesh point
farthest.distance = d2;
}
}
} // for j
} // for i
if (!found_a_real && found_a_NAN) { // if the mesh is totally unpopulated, start the probing
out_mesh.x_index = GRID_MAX_POINTS_X / 2;
out_mesh.y_index = GRID_MAX_POINTS_Y / 2;
out_mesh.distance = 1;
farthest.pos.set(GRID_MAX_POINTS_X / 2, GRID_MAX_POINTS_Y / 2);
farthest.distance = 1;
}
return out_mesh;
return farthest;
}
mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const float &rx, const float &ry, const bool probe_as_reference, uint16_t bits[16]) {
mesh_index_pair out_mesh;
out_mesh.x_index = out_mesh.y_index = -1;
out_mesh.distance = -99999.9f;
mesh_index_pair unified_bed_leveling::find_closest_mesh_point_of_type(const MeshPointType type, const xy_pos_t &pos, const bool probe_relative/*=false*/, MeshFlags *done_flags/*=nullptr*/) {
mesh_index_pair closest;
closest.invalidate();
closest.distance = -99999.9f;
// Get our reference position. Either the nozzle or probe location.
const float px = rx + (probe_as_reference == USE_PROBE_AS_REFERENCE ? probe_offset[X_AXIS] : 0),
py = ry + (probe_as_reference == USE_PROBE_AS_REFERENCE ? probe_offset[Y_AXIS] : 0);
// Get the reference position, either nozzle or probe
const xy_pos_t ref = probe_relative ? pos + probe_offset : pos;
float best_so_far = 99999.99f;
for (int8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (int8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
if ( (type == INVALID && isnan(z_values[i][j])) // Check to see if this location holds the right thing
|| (type == REAL && !isnan(z_values[i][j]))
|| (type == SET_IN_BITMAP && is_bitmap_set(bits, i, j))
if ( (type == (isnan(z_values[i][j]) ? INVALID : REAL))
|| (type == SET_IN_BITMAP && !done_flags->marked(i, j))
) {
// We only get here if we found a Mesh Point of the specified type
const float mx = mesh_index_to_xpos(i),
my = mesh_index_to_ypos(j);
// Found a Mesh Point of the specified type!
const xy_pos_t mpos = { mesh_index_to_xpos(i), mesh_index_to_ypos(j) };
// If using the probe as the reference there are some unreachable locations.
// Also for round beds, there are grid points outside the bed the nozzle can't reach.
// Prune them from the list and ignore them till the next Phase (manual nozzle probing).
if (probe_as_reference ? !position_is_reachable_by_probe(mx, my) : !position_is_reachable(mx, my))
if (probe_relative ? !position_is_reachable_by_probe(mpos) : !position_is_reachable(mpos))
continue;
// Reachable. Check if it's the best_so_far location to the nozzle.
float distance = HYPOT(px - mx, py - my);
const xy_pos_t diff = current_position - mpos;
const float distance = (ref - mpos).magnitude() + diff.magnitude() * 0.1f;
// factor in the distance from the current location for the normal case
// so the nozzle isn't running all over the bed.
distance += HYPOT(current_position[X_AXIS] - mx, current_position[Y_AXIS] - my) * 0.1f;
if (distance < best_so_far) {
best_so_far = distance; // We found a closer location with
out_mesh.x_index = i; // the specified type of mesh value.
out_mesh.y_index = j;
out_mesh.distance = best_so_far;
best_so_far = distance; // Found a closer location with the desired value type.
closest.pos.set(i, j);
closest.distance = best_so_far;
}
}
} // for j
} // for i
return out_mesh;
return closest;
}
/**
@ -1332,20 +1326,20 @@
*/
bool unified_bed_leveling::smart_fill_one(const uint8_t x, const uint8_t y, const int8_t xdir, const int8_t ydir) {
const int8_t x1 = x + xdir, x2 = x1 + xdir,
y1 = y + ydir, y2 = y1 + ydir;
// A NAN next to a pair of real values?
if (isnan(z_values[x][y]) && !isnan(z_values[x1][y1]) && !isnan(z_values[x2][y2])) {
if (z_values[x1][y1] < z_values[x2][y2]) // Angled downward?
z_values[x][y] = z_values[x1][y1]; // Use nearest (maybe a little too high.)
else
z_values[x][y] = 2.0f * z_values[x1][y1] - z_values[x2][y2]; // Angled upward...
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(x, y, z_values[x][y]);
#endif
return true;
const float v = z_values[x][y];
if (isnan(v)) { // A NAN...
const int8_t dx = x + xdir, dy = y + ydir;
const float v1 = z_values[dx][dy];
if (!isnan(v1)) { // ...next to a pair of real values?
const float v2 = z_values[dx + xdir][dy + ydir];
if (!isnan(v2)) {
z_values[x][y] = v1 < v2 ? v1 : v1 + v1 - v2;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(x, y, z_values[pos.x][pos.y]);
#endif
return true;
}
}
}
return false;
}
@ -1391,15 +1385,15 @@
dx = (x_max - x_min) / (g29_grid_size - 1),
dy = (y_max - y_min) / (g29_grid_size - 1);
vector_3 points[3] = {
const vector_3 points[3] = {
#if ENABLED(HAS_FIXED_3POINT)
vector_3(PROBE_PT_1_X, PROBE_PT_1_Y, 0),
vector_3(PROBE_PT_2_X, PROBE_PT_2_Y, 0),
vector_3(PROBE_PT_3_X, PROBE_PT_3_Y, 0)
{ PROBE_PT_1_X, PROBE_PT_1_Y, 0 },
{ PROBE_PT_2_X, PROBE_PT_2_Y, 0 },
{ PROBE_PT_3_X, PROBE_PT_3_Y, 0 }
#else
vector_3(x_min, y_min, 0),
vector_3(x_max, y_min, 0),
vector_3((x_max - x_min) / 2, y_max, 0)
{ x_min, y_min, 0 },
{ x_max, y_min, 0 },
{ (x_max - x_min) / 2, y_max, 0 }
#endif
};
@ -1419,11 +1413,11 @@
ui.status_printf_P(0, PSTR(MSG_LCD_TILTING_MESH " 1/3"));
#endif
measured_z = probe_at_point(points[0].x, points[0].y, PROBE_PT_RAISE, g29_verbose_level);
measured_z = probe_at_point(points[0], PROBE_PT_RAISE, g29_verbose_level);
if (isnan(measured_z))
abort_flag = true;
else {
measured_z -= get_z_correction(points[0].x, points[0].y);
measured_z -= get_z_correction(points[0]);
#ifdef VALIDATE_MESH_TILT
z1 = measured_z;
#endif
@ -1431,7 +1425,7 @@
serial_spaces(16);
SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
}
incremental_LSF(&lsf_results, points[0].x, points[0].y, measured_z);
incremental_LSF(&lsf_results, points[0], measured_z);
}
if (!abort_flag) {
@ -1440,19 +1434,19 @@
ui.status_printf_P(0, PSTR(MSG_LCD_TILTING_MESH " 2/3"));
#endif
measured_z = probe_at_point(points[1].x, points[1].y, PROBE_PT_RAISE, g29_verbose_level);
measured_z = probe_at_point(points[1], PROBE_PT_RAISE, g29_verbose_level);
#ifdef VALIDATE_MESH_TILT
z2 = measured_z;
#endif
if (isnan(measured_z))
abort_flag = true;
else {
measured_z -= get_z_correction(points[1].x, points[1].y);
measured_z -= get_z_correction(points[1]);
if (g29_verbose_level > 3) {
serial_spaces(16);
SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
}
incremental_LSF(&lsf_results, points[1].x, points[1].y, measured_z);
incremental_LSF(&lsf_results, points[1], measured_z);
}
}
@ -1462,19 +1456,19 @@
ui.status_printf_P(0, PSTR(MSG_LCD_TILTING_MESH " 3/3"));
#endif
measured_z = probe_at_point(points[2].x, points[2].y, PROBE_PT_STOW, g29_verbose_level);
measured_z = probe_at_point(points[2], PROBE_PT_STOW, g29_verbose_level);
#ifdef VALIDATE_MESH_TILT
z3 = measured_z;
#endif
if (isnan(measured_z))
abort_flag = true;
else {
measured_z -= get_z_correction(points[2].x, points[2].y);
measured_z -= get_z_correction(points[2]);
if (g29_verbose_level > 3) {
serial_spaces(16);
SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
}
incremental_LSF(&lsf_results, points[2].x, points[2].y, measured_z);
incremental_LSF(&lsf_results, points[2], measured_z);
}
}
@ -1494,10 +1488,11 @@
uint16_t total_points = g29_grid_size * g29_grid_size, point_num = 1;
xy_pos_t rpos;
for (uint8_t ix = 0; ix < g29_grid_size; ix++) {
const float rx = x_min + ix * dx;
rpos.x = x_min + ix * dx;
for (int8_t iy = 0; iy < g29_grid_size; iy++) {
const float ry = y_min + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
rpos.y = y_min + dy * (zig_zag ? g29_grid_size - 1 - iy : iy);
if (!abort_flag) {
SERIAL_ECHOLNPAIR("Tilting mesh point ", point_num, "/", total_points, "\n");
@ -1505,24 +1500,24 @@
ui.status_printf_P(0, PSTR(MSG_LCD_TILTING_MESH " %i/%i"), point_num, total_points);
#endif
measured_z = probe_at_point(rx, ry, parser.seen('E') ? PROBE_PT_STOW : PROBE_PT_RAISE, g29_verbose_level); // TODO: Needs error handling
measured_z = probe_at_point(rpos, parser.seen('E') ? PROBE_PT_STOW : PROBE_PT_RAISE, g29_verbose_level); // TODO: Needs error handling
abort_flag = isnan(measured_z);
if (DEBUGGING(LEVELING)) {
const xy_pos_t lpos = rpos.asLogical();
DEBUG_CHAR('(');
DEBUG_ECHO_F(rx, 7);
DEBUG_ECHO_F(rpos.x, 7);
DEBUG_CHAR(',');
DEBUG_ECHO_F(ry, 7);
DEBUG_ECHOPGM(") logical: (");
DEBUG_ECHO_F(LOGICAL_X_POSITION(rx), 7);
DEBUG_ECHO_F(rpos.y, 7);
DEBUG_ECHOPAIR_F(") logical: (", lpos.x, 7);
DEBUG_CHAR(',');
DEBUG_ECHO_F(LOGICAL_Y_POSITION(ry), 7);
DEBUG_ECHO_F(lpos.y, 7);
DEBUG_ECHOPAIR_F(") measured: ", measured_z, 7);
DEBUG_ECHOPAIR_F(" correction: ", get_z_correction(rx, ry), 7);
DEBUG_ECHOPAIR_F(" correction: ", get_z_correction(rpos), 7);
}
measured_z -= get_z_correction(rx, ry) /* + probe_offset[Z_AXIS] */ ;
measured_z -= get_z_correction(rpos) /* + probe_offset.z */ ;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR_F(" final >>>---> ", measured_z, 7);
@ -1530,7 +1525,7 @@
serial_spaces(16);
SERIAL_ECHOLNPAIR("Corrected_Z=", measured_z);
}
incremental_LSF(&lsf_results, rx, ry, measured_z);
incremental_LSF(&lsf_results, rpos, measured_z);
}
point_num++;
@ -1564,33 +1559,33 @@
for (uint8_t i = 0; i < GRID_MAX_POINTS_X; i++) {
for (uint8_t j = 0; j < GRID_MAX_POINTS_Y; j++) {
float x_tmp = mesh_index_to_xpos(i),
y_tmp = mesh_index_to_ypos(j),
z_tmp = z_values[i][j];
float mx = mesh_index_to_xpos(i),
my = mesh_index_to_ypos(j),
mz = z_values[i][j];
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR_F("before rotation = [", x_tmp, 7);
DEBUG_ECHOPAIR_F("before rotation = [", mx, 7);
DEBUG_CHAR(',');
DEBUG_ECHO_F(y_tmp, 7);
DEBUG_ECHO_F(my, 7);
DEBUG_CHAR(',');
DEBUG_ECHO_F(z_tmp, 7);
DEBUG_ECHO_F(mz, 7);
DEBUG_ECHOPGM("] ---> ");
DEBUG_DELAY(20);
}
apply_rotation_xyz(rotation, x_tmp, y_tmp, z_tmp);
apply_rotation_xyz(rotation, mx, my, mz);
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR_F("after rotation = [", x_tmp, 7);
DEBUG_ECHOPAIR_F("after rotation = [", mx, 7);
DEBUG_CHAR(',');
DEBUG_ECHO_F(y_tmp, 7);
DEBUG_ECHO_F(my, 7);
DEBUG_CHAR(',');
DEBUG_ECHO_F(z_tmp, 7);
DEBUG_ECHO_F(mz, 7);
DEBUG_ECHOLNPGM("]");
DEBUG_DELAY(55);
DEBUG_DELAY(20);
}
z_values[i][j] = z_tmp - lsf_results.D;
z_values[i][j] = mz - lsf_results.D;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(i, j, z_values[i][j]);
#endif
@ -1613,41 +1608,32 @@
DEBUG_EOL();
/**
* The following code can be used to check the validity of the mesh tilting algorithm.
* When a 3-Point Mesh Tilt is done, the same algorithm is used as the grid based tilting.
* The only difference is just 3 points are used in the calculations. That fact guarantees
* each probed point should have an exact match when a get_z_correction() for that location
* is calculated. The Z error between the probed point locations and the get_z_correction()
* Use the code below to check the validity of the mesh tilting algorithm.
* 3-Point Mesh Tilt uses the same algorithm as grid-based tilting, but only
* three points are used in the calculation. This guarantees that each probed point
* has an exact match when get_z_correction() for that location is calculated.
* The Z error between the probed point locations and the get_z_correction()
* numbers for those locations should be 0.
*/
#ifdef VALIDATE_MESH_TILT
float t, t1, d;
t = normal.x * x_min + normal.y * y_min;
d = t + normal.z * z1;
DEBUG_ECHOPAIR_F("D from 1st point: ", d, 6);
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z * z1 - get_z_correction(x_min, y_min), 6);
t = normal.x * x_max + normal.y * y_min;
d = t + normal.z * z2;
DEBUG_EOL();
DEBUG_ECHOPAIR_F("D from 2nd point: ", d, 6);
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z * z2 - get_z_correction(x_max, y_min), 6);
t = normal.x * ((x_max - x_min) / 2) + normal.y * (y_min);
d = t + normal.z * z3;
DEBUG_ECHOPAIR_F("D from 3rd point: ", d, 6);
DEBUG_ECHOLNPAIR_F(" Z error: ", normal.z * z3 - get_z_correction((x_max - x_min) / 2, y_max), 6);
t = normal.x * (Z_SAFE_HOMING_X_POINT) + normal.y * (Z_SAFE_HOMING_Y_POINT);
d = t + normal.z * 0;
DEBUG_ECHOLNPAIR_F("D from home location with Z=0 : ", d, 6);
t = normal.x * (Z_SAFE_HOMING_X_POINT) + normal.y * (Z_SAFE_HOMING_Y_POINT);
d = t + get_z_correction(Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT); // normal.z * 0;
DEBUG_ECHOPAIR_F("D from home location using mesh value for Z: ", d, 6);
auto d_from = []() { DEBUG_ECHOPGM("D from "); };
auto normed = [&](const xy_pos_t &pos, const float &zadd) {
return normal.x * pos.x + normal.y * pos.y + zadd;
};
auto debug_pt = [](PGM_P const pre, const xy_pos_t &pos, const float &zadd) {
d_from(); serialprintPGM(pre);
DEBUG_ECHO_F(normed(pos, zadd), 6);
DEBUG_ECHOLNPAIR_F(" Z error: ", zadd - get_z_correction(pos), 6);
};
debug_pt(PSTR("1st point: "), probe_pt[0], normal.z * z1);
debug_pt(PSTR("2nd point: "), probe_pt[1], normal.z * z2);
debug_pt(PSTR("3rd point: "), probe_pt[2], normal.z * z3);
d_from(); DEBUG_ECHOPGM("safe home with Z=");
DEBUG_ECHOLNPAIR_F("0 : ", normed(safe_homing_xy, 0), 6);
d_from(); DEBUG_ECHOPGM("safe home with Z=");
DEBUG_ECHOLNPAIR_F("mesh value ", normed(safe_homing_xy, get_z_correction(safe_homing_xy)), 6);
DEBUG_ECHOPAIR(" Z error: (", Z_SAFE_HOMING_X_POINT, ",", Z_SAFE_HOMING_Y_POINT);
DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(Z_SAFE_HOMING_X_POINT, Z_SAFE_HOMING_Y_POINT), 6);
DEBUG_ECHOLNPAIR_F(") = ", get_z_correction(safe_homing_xy), 6);
#endif
} // DEBUGGING(LEVELING)
@ -1676,21 +1662,23 @@
if (!isnan(z_values[jx][jy]))
SBI(bitmap[jx], jy);
xy_pos_t ppos;
for (uint8_t ix = 0; ix < GRID_MAX_POINTS_X; ix++) {
const float px = mesh_index_to_xpos(ix);
ppos.x = mesh_index_to_xpos(ix);
for (uint8_t iy = 0; iy < GRID_MAX_POINTS_Y; iy++) {
const float py = mesh_index_to_ypos(iy);
ppos.y = mesh_index_to_ypos(iy);
if (isnan(z_values[ix][iy])) {
// undefined mesh point at (px,py), compute weighted LSF from original valid mesh points.
// undefined mesh point at (ppos.x,ppos.y), compute weighted LSF from original valid mesh points.
incremental_LSF_reset(&lsf_results);
xy_pos_t rpos;
for (uint8_t jx = 0; jx < GRID_MAX_POINTS_X; jx++) {
const float rx = mesh_index_to_xpos(jx);
rpos.x = mesh_index_to_xpos(jx);
for (uint8_t jy = 0; jy < GRID_MAX_POINTS_Y; jy++) {
if (TEST(bitmap[jx], jy)) {
const float ry = mesh_index_to_ypos(jy),
rz = z_values[jx][jy],
w = 1 + weight_scaled / HYPOT((rx - px), (ry - py));
incremental_WLSF(&lsf_results, rx, ry, rz, w);
rpos.y = mesh_index_to_ypos(jy);
const float rz = z_values[jx][jy],
w = 1.0f + weight_scaled / (rpos - ppos).magnitude();
incremental_WLSF(&lsf_results, rpos, rz, w);
}
}
}
@ -1698,12 +1686,12 @@
SERIAL_ECHOLNPGM("Insufficient data");
return;
}
const float ez = -lsf_results.D - lsf_results.A * px - lsf_results.B * py;
const float ez = -lsf_results.D - lsf_results.A * ppos.x - lsf_results.B * ppos.y;
z_values[ix][iy] = ez;
#if ENABLED(EXTENSIBLE_UI)
ExtUI::onMeshUpdate(ix, iy, z_values[ix][iy]);
#endif
idle(); // housekeeping
idle(); // housekeeping
}
}
}
@ -1734,7 +1722,7 @@
adjust_mesh_to_mean(g29_c_flag, g29_constant);
#if HAS_BED_PROBE
SERIAL_ECHOLNPAIR_F("Probe Offset M851 Z", probe_offset[Z_AXIS], 7);
SERIAL_ECHOLNPAIR_F("Probe Offset M851 Z", probe_offset.z, 7);
#endif
SERIAL_ECHOLNPAIR("MESH_MIN_X " STRINGIFY(MESH_MIN_X) "=", MESH_MIN_X); serial_delay(50);

355
Marlin/src/feature/bedlevel/ubl/ubl_motion.cpp
View File

@ -35,12 +35,6 @@
#include "../../../Marlin.h"
#include <math.h>
#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
void unified_bed_leveling::line_to_destination_cartesian(const feedRate_t &scaled_fr_mm_s, const uint8_t extruder) {
@ -50,60 +44,57 @@
* just do the required Z-Height correction, call the Planner's buffer_line() routine, and leave
*/
#if HAS_POSITION_MODIFIERS
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] };
xyze_pos_t start = current_position, end = destination;
planner.apply_modifiers(start);
planner.apply_modifiers(end);
#else
const float (&start)[XYZE] = current_position,
(&end)[XYZE] = destination;
const xyze_pos_t &start = current_position, &end = destination;
#endif
const int cell_start_xi = get_cell_index_x(start[X_AXIS]),
cell_start_yi = get_cell_index_y(start[Y_AXIS]),
cell_dest_xi = get_cell_index_x(end[X_AXIS]),
cell_dest_yi = get_cell_index_y(end[Y_AXIS]);
const xy_int8_t istart = cell_indexes(start), iend = cell_indexes(end);
// A move within the same cell needs no splitting
if (cell_start_xi == cell_dest_xi && cell_start_yi == cell_dest_yi) {
if (istart == iend) {
// For a move off the bed, use a constant Z raise
if (!WITHIN(cell_dest_xi, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(cell_dest_yi, 0, GRID_MAX_POINTS_Y - 1)) {
if (!WITHIN(iend.x, 0, GRID_MAX_POINTS_X - 1) || !WITHIN(iend.y, 0, GRID_MAX_POINTS_Y - 1)) {
// Note: There is no Z Correction in this case. We are off the grid and don't know what
// a reasonable correction would be. If the user has specified a UBL_Z_RAISE_WHEN_OFF_MESH
// value, that will be used instead of a calculated (Bi-Linear interpolation) correction.
const float z_raise = 0.0
#ifdef UBL_Z_RAISE_WHEN_OFF_MESH
+ UBL_Z_RAISE_WHEN_OFF_MESH
#endif
;
planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + z_raise, end[E_AXIS], scaled_fr_mm_s, extruder);
set_current_from_destination();
#ifdef UBL_Z_RAISE_WHEN_OFF_MESH
end.z += UBL_Z_RAISE_WHEN_OFF_MESH;
#endif
planner.buffer_segment(end, scaled_fr_mm_s, extruder);
current_position = destination;
return;
}
FINAL_MOVE:
// The distance is always MESH_X_DIST so multiply by the constant reciprocal.
const float xratio = (end[X_AXIS] - mesh_index_to_xpos(cell_dest_xi)) * RECIPROCAL(MESH_X_DIST);
const float xratio = (end.x - mesh_index_to_xpos(iend.x)) * RECIPROCAL(MESH_X_DIST);
float z1 = z_values[cell_dest_xi ][cell_dest_yi ] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi ] - z_values[cell_dest_xi][cell_dest_yi ]),
z2 = z_values[cell_dest_xi ][cell_dest_yi + 1] + xratio *
(z_values[cell_dest_xi + 1][cell_dest_yi + 1] - z_values[cell_dest_xi][cell_dest_yi + 1]);
if (cell_dest_xi >= GRID_MAX_POINTS_X - 1) z1 = z2 = 0.0;
float z1, z2;
if (iend.x >= GRID_MAX_POINTS_X - 1)
z1 = z2 = 0.0;
else {
z1 = z_values[iend.x ][iend.y ] + xratio *
(z_values[iend.x + 1][iend.y ] - z_values[iend.x][iend.y ]),
z2 = z_values[iend.x ][iend.y + 1] + xratio *
(z_values[iend.x + 1][iend.y + 1] - z_values[iend.x][iend.y + 1]);
}
// X cell-fraction done. Interpolate the two Z offsets with the Y fraction for the final Z offset.
const float yratio = (end[Y_AXIS] - mesh_index_to_ypos(cell_dest_yi)) * RECIPROCAL(MESH_Y_DIST),
z0 = cell_dest_yi < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end[Z_AXIS]) : 0.0;
const float yratio = (end.y - mesh_index_to_ypos(iend.y)) * RECIPROCAL(MESH_Y_DIST),
z0 = iend.y < GRID_MAX_POINTS_Y - 1 ? (z1 + (z2 - z1) * yratio) * planner.fade_scaling_factor_for_z(end.z) : 0.0;
// Undefined parts of the Mesh in z_values[][] are NAN.
// Replace NAN corrections with 0.0 to prevent NAN propagation.
planner.buffer_segment(end[X_AXIS], end[Y_AXIS], end[Z_AXIS] + (isnan(z0) ? 0.0 : z0), end[E_AXIS], scaled_fr_mm_s, extruder);
set_current_from_destination();
if (!isnan(z0)) end.z += z0;
planner.buffer_segment(end, scaled_fr_mm_s, extruder);
current_position = destination;
return;
}
@ -112,17 +103,11 @@
* case - crossing only one X or Y line - after details are worked out to reduce computation.
*/
const float dx = end[X_AXIS] - start[X_AXIS],
dy = end[Y_AXIS] - start[Y_AXIS];
const int left_flag = dx < 0.0 ? 1 : 0,
down_flag = dy < 0.0 ? 1 : 0;
const float adx = left_flag ? -dx : dx,
ady = down_flag ? -dy : dy;
const int dxi = cell_start_xi == cell_dest_xi ? 0 : left_flag ? -1 : 1,
dyi = cell_start_yi == cell_dest_yi ? 0 : down_flag ? -1 : 1;
const xy_float_t dist = end - start;
const xy_bool_t neg { dist.x < 0, dist.y < 0 };
const xy_int8_t ineg { int8_t(neg.x), int8_t(neg.y) };
const xy_float_t sign { neg.x ? -1.0f : 1.0f, neg.y ? -1.0f : 1.0f };
const xy_int8_t iadd { int8_t(iend.x == istart.x ? 0 : sign.x), int8_t(iend.y == istart.y ? 0 : sign.y) };
/**
* Compute the extruder scaling factor for each partial move, checking for
@ -132,64 +117,64 @@
* components. The larger of the two is used to preserve precision.
*/
const bool use_x_dist = adx > ady;
const xy_float_t ad = sign * dist;
const bool use_x_dist = ad.x > ad.y;
float on_axis_distance = use_x_dist ? dx : dy,
e_position = end[E_AXIS] - start[E_AXIS],
z_position = end[Z_AXIS] - start[Z_AXIS];
float on_axis_distance = use_x_dist ? dist.x : dist.y,
e_position = end.e - start.e,
z_position = end.z - start.z;
const float e_normalized_dist = e_position / on_axis_distance,
const float e_normalized_dist = e_position / on_axis_distance, // Allow divide by zero
z_normalized_dist = z_position / on_axis_distance;
int current_xi = cell_start_xi,
current_yi = cell_start_yi;
xy_int8_t icell = istart;
const float m = dy / dx,
c = start[Y_AXIS] - m * start[X_AXIS];
const float ratio = dist.y / dist.x, // Allow divide by zero
c = start.y - ratio * start.x;
const bool inf_normalized_flag = (isinf(e_normalized_dist) != 0),
inf_m_flag = (isinf(m) != 0);
const bool inf_normalized_flag = isinf(e_normalized_dist),
inf_ratio_flag = isinf(ratio);
/**
* Handle vertical lines that stay within one column.
* These need not be perfectly vertical.
*/
if (dxi == 0) { // Vertical line?
current_yi += down_flag; // Line going down? Just go to the bottom.
while (current_yi != cell_dest_yi + down_flag) {
current_yi += dyi;
const float next_mesh_line_y = mesh_index_to_ypos(current_yi);
if (iadd.x == 0) { // Vertical line?
icell.y += ineg.y; // Line going down? Just go to the bottom.
while (icell.y != iend.y + ineg.y) {
icell.y += iadd.y;
const float next_mesh_line_y = mesh_index_to_ypos(icell.y);
/**
* Skip the calculations for an infinite slope.
* For others the next X is the same so this can continue.
* Calculate X at the next Y mesh line.
*/
const float rx = inf_m_flag ? start[X_AXIS] : (next_mesh_line_y - c) / m;
const float rx = inf_ratio_flag ? start.x : (next_mesh_line_y - c) / ratio;
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, current_xi, current_yi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, icell.x, icell.y)
* planner.fade_scaling_factor_for_z(end.z);
// Undefined parts of the Mesh in z_values[][] are NAN.
// Replace NAN corrections with 0.0 to prevent NAN propagation.
if (isnan(z0)) z0 = 0.0;
const float ry = mesh_index_to_ypos(current_yi);
const float ry = mesh_index_to_ypos(icell.y);
/**
* Without this check, it's possible to generate a zero length move, as in the case where
* the line is heading down, starting exactly on a mesh line boundary. Since this is rare
* it might be fine to remove this check and let planner.buffer_segment() filter it out.
*/
if (ry != start[Y_AXIS]) {
if (!inf_normalized_flag) {
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;
if (ry != start.y) {
if (!inf_normalized_flag) { // fall-through faster than branch
on_axis_distance = use_x_dist ? rx - start.x : ry - start.y;
e_position = start.e + on_axis_distance * e_normalized_dist;
z_position = start.z + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
e_position = end.e;
z_position = end.z;
}
planner.buffer_segment(rx, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder);
@ -197,10 +182,10 @@
}
// At the final destination? Usually not, but when on a Y Mesh Line it's completed.
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
if (xy_pos_t(current_position) != xy_pos_t(end))
goto FINAL_MOVE;
set_current_from_destination();
current_position = destination;
return;
}
@ -208,36 +193,34 @@
* Handle horizontal lines that stay within one row.
* These need not be perfectly horizontal.
*/
if (dyi == 0) { // Horizontal line?
current_xi += left_flag; // Heading left? Just go to the left edge of the cell for the first move.
while (current_xi != cell_dest_xi + left_flag) {
current_xi += dxi;
const float next_mesh_line_x = mesh_index_to_xpos(current_xi),
ry = m * next_mesh_line_x + c; // Calculate Y at the next X mesh line
if (iadd.y == 0) { // Horizontal line?
icell.x += ineg.x; // Heading left? Just go to the left edge of the cell for the first move.
while (icell.x != iend.x + ineg.x) {
icell.x += iadd.x;
const float rx = mesh_index_to_xpos(icell.x);
const float ry = ratio * rx + c; // Calculate Y at the next X mesh line
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, current_xi, current_yi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, icell.x, icell.y)
* planner.fade_scaling_factor_for_z(end.z);
// Undefined parts of the Mesh in z_values[][] are NAN.
// Replace NAN corrections with 0.0 to prevent NAN propagation.
if (isnan(z0)) z0 = 0.0;
const float rx = mesh_index_to_xpos(current_xi);
/**
* Without this check, it's possible to generate a zero length move, as in the case where
* the line is heading left, starting exactly on a mesh line boundary. Since this is rare
* it might be fine to remove this check and let planner.buffer_segment() filter it out.
*/
if (rx != start[X_AXIS]) {
if (rx != start.x) {
if (!inf_normalized_flag) {
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;
on_axis_distance = use_x_dist ? rx - start.x : ry - start.y;
e_position = start.e + on_axis_distance * e_normalized_dist; // is based on X or Y because this is a horizontal move
z_position = start.z + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
e_position = end.e;
z_position = end.z;
}
if (!planner.buffer_segment(rx, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder))
@ -245,93 +228,88 @@
} //else printf("FIRST MOVE PRUNED ");
}
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
if (xy_pos_t(current_position) != xy_pos_t(end))
goto FINAL_MOVE;
set_current_from_destination();
current_position = destination;
return;
}
/**
*
* Handle the generic case of a line crossing both X and Y Mesh lines.
* Generic case of a line crossing both X and Y Mesh lines.
*
*/
int xi_cnt = cell_start_xi - cell_dest_xi,
yi_cnt = cell_start_yi - cell_dest_yi;
xy_int8_t cnt = (istart - iend).ABS();
if (xi_cnt < 0) xi_cnt = -xi_cnt;
if (yi_cnt < 0) yi_cnt = -yi_cnt;
icell += ineg;
current_xi += left_flag;
current_yi += down_flag;
while (cnt) {
while (xi_cnt || yi_cnt) {
const float next_mesh_line_x = mesh_index_to_xpos(icell.x + iadd.x),
next_mesh_line_y = mesh_index_to_ypos(icell.y + iadd.y),
ry = ratio * next_mesh_line_x + c, // Calculate Y at the next X mesh line
rx = (next_mesh_line_y - c) / ratio; // Calculate X at the next Y mesh line
// (No need to worry about ratio == 0.
// In that case, it was already detected
// as a vertical line move above.)
const float next_mesh_line_x = mesh_index_to_xpos(current_xi + dxi),
next_mesh_line_y = mesh_index_to_ypos(current_yi + dyi),
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 == (rx > next_mesh_line_x)) { // Check if we hit the Y line first
if (neg.x == (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(rx, current_xi - left_flag, current_yi + dyi)
* planner.fade_scaling_factor_for_z(end[Z_AXIS]);
float z0 = z_correction_for_x_on_horizontal_mesh_line(rx, icell.x - ineg.x, icell.y + iadd.y)
* planner.fade_scaling_factor_for_z(end.z);
// Undefined parts of the Mesh in z_values[][] are NAN.
// Replace NAN corrections with 0.0 to prevent NAN propagation.
if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) {
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;
on_axis_distance = use_x_dist ? rx - start.x : next_mesh_line_y - start.y;
e_position = start.e + on_axis_distance * e_normalized_dist;
z_position = start.z + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
e_position = end.e;
z_position = end.z;
}
if (!planner.buffer_segment(rx, next_mesh_line_y, z_position + z0, e_position, scaled_fr_mm_s, extruder))
break;
current_yi += dyi;
yi_cnt--;
icell.y += iadd.y;
cnt.y--;
}
else {
// Yes! Crossing a X Mesh Line next
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]);
float z0 = z_correction_for_y_on_vertical_mesh_line(ry, icell.x + iadd.x, icell.y - ineg.y)
* planner.fade_scaling_factor_for_z(end.z);
// Undefined parts of the Mesh in z_values[][] are NAN.
// Replace NAN corrections with 0.0 to prevent NAN propagation.
if (isnan(z0)) z0 = 0.0;
if (!inf_normalized_flag) {
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;
on_axis_distance = use_x_dist ? next_mesh_line_x - start.x : ry - start.y;
e_position = start.e + on_axis_distance * e_normalized_dist;
z_position = start.z + on_axis_distance * z_normalized_dist;
}
else {
e_position = end[E_AXIS];
z_position = end[Z_AXIS];
e_position = end.e;
z_position = end.z;
}
if (!planner.buffer_segment(next_mesh_line_x, ry, z_position + z0, e_position, scaled_fr_mm_s, extruder))
break;
current_xi += dxi;
xi_cnt--;
icell.x += iadd.x;
cnt.x--;
}
if (xi_cnt < 0 || yi_cnt < 0) break; // Too far! Exit the loop and go to FINAL_MOVE
if (cnt.x < 0 || cnt.y < 0) break; // Too far! Exit the loop and go to FINAL_MOVE
}
if (current_position[X_AXIS] != end[X_AXIS] || current_position[Y_AXIS] != end[Y_AXIS])
if (xy_pos_t(current_position) != xy_pos_t(end))
goto FINAL_MOVE;
set_current_from_destination();
current_position = destination;
}
#else // UBL_SEGMENTED
@ -356,56 +334,42 @@
bool _O2 unified_bed_leveling::line_to_destination_segmented(const feedRate_t &scaled_fr_mm_s) {
if (!position_is_reachable(destination[X_AXIS], destination[Y_AXIS])) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
if (!position_is_reachable(destination)) // fail if moving outside reachable boundary
return true; // did not move, so current_position still accurate
const float total[XYZE] = {
destination[X_AXIS] - current_position[X_AXIS],
destination[Y_AXIS] - current_position[Y_AXIS],
destination[Z_AXIS] - current_position[Z_AXIS],
destination[E_AXIS] - current_position[E_AXIS]
};
const xyze_pos_t total = destination - current_position;
const float cartesian_xy_mm = HYPOT(total[X_AXIS], total[Y_AXIS]); // total horizontal xy distance
const float cart_xy_mm_2 = HYPOT2(total.x, total.y),
cart_xy_mm = SQRT(cart_xy_mm_2); // Total XY distance
#if IS_KINEMATIC
const float seconds = cartesian_xy_mm / scaled_fr_mm_s; // Duration of XY move at requested rate
uint16_t segments = LROUND(delta_segments_per_second * seconds), // Preferred number of segments for distance @ feedrate
seglimit = LROUND(cartesian_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Number of segments at minimum segment length
NOMORE(segments, seglimit); // Limit to minimum segment length (fewer segments)
const float seconds = cart_xy_mm / scaled_fr_mm_s; // Duration of XY move at requested rate
uint16_t segments = LROUND(delta_segments_per_second * seconds), // Preferred number of segments for distance @ feedrate
seglimit = LROUND(cart_xy_mm * RECIPROCAL(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 = LROUND(cartesian_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // cartesian fixed segment length
uint16_t segments = LROUND(cart_xy_mm * RECIPROCAL(DELTA_SEGMENT_MIN_LENGTH)); // Cartesian fixed segment length
#endif
NOLESS(segments, 1U); // must have at least one segment
const float inv_segments = 1.0f / segments; // divide once, multiply thereafter
NOLESS(segments, 1U); // Must have at least one segment
const float inv_segments = 1.0f / segments, // Reciprocal to save calculation
segment_xyz_mm = SQRT(cart_xy_mm_2 + sq(total.z)) * inv_segments; // Length of each segment
const float segment_xyz_mm = HYPOT(cartesian_xy_mm, total[Z_AXIS]) * inv_segments; // length of each segment
#if ENABLED(SCARA_FEEDRATE_SCALING)
const float inv_duration = scaled_fr_mm_s / segment_xyz_mm;
#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
};
xyze_float_t diff = total * 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]
};
xyze_pos_t raw = current_position;
// Just do plain segmentation if UBL is inactive or the target is above the fade height
if (!planner.leveling_active || !planner.leveling_active_at_z(destination[Z_AXIS])) {
if (!planner.leveling_active || !planner.leveling_active_at_z(destination.z)) {
while (--segments) {
LOOP_XYZE(i) raw[i] += diff[i];
raw += diff;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
@ -417,17 +381,17 @@
, inv_duration
#endif
);
return false; // moved but did not set_current_from_destination();
return false; // Did not set current from destination
}
// Otherwise perform per-segment leveling
#if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
const float fade_scaling_factor = planner.fade_scaling_factor_for_z(destination[Z_AXIS]);
const float fade_scaling_factor = planner.fade_scaling_factor_for_z(destination.z);
#endif
// increment to first segment destination
LOOP_XYZE(i) raw[i] += diff[i];
// Move to first segment destination
raw += diff;
for (;;) { // for each mesh cell encountered during the move
@ -438,75 +402,68 @@
// 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)) * RECIPROCAL(MESH_X_DIST),
cell_yi = (raw[Y_AXIS] - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST);
xy_int8_t icell = {
int8_t((raw.x - (MESH_MIN_X)) * RECIPROCAL(MESH_X_DIST)),
int8_t((raw.y - (MESH_MIN_Y)) * RECIPROCAL(MESH_Y_DIST))
};
LIMIT(icell.x, 0, (GRID_MAX_POINTS_X) - 1);
LIMIT(icell.y, 0, (GRID_MAX_POINTS_Y) - 1);
LIMIT(cell_xi, 0, (GRID_MAX_POINTS_X) - 1);
LIMIT(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
float z_x0y0 = z_values[icell.x ][icell.y ], // z at lower left corner
z_x1y0 = z_values[icell.x+1][icell.y ], // z at upper left corner
z_x0y1 = z_values[icell.x ][icell.y+1], // z at lower right corner
z_x1y1 = z_values[icell.x+1][icell.y+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 xy_pos_t pos = { mesh_index_to_xpos(icell.x), mesh_index_to_ypos(icell.y) };
xy_pos_t cell = raw - pos;
const float z_xmy0 = (z_x1y0 - z_x0y0) * RECIPROCAL(MESH_X_DIST), // z slope per x along y0 (lower left to lower right)
z_xmy1 = (z_x1y1 - z_x0y1) * RECIPROCAL(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 * cell.x; // z height along y0 at cell.x (changes for each cell.x 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 * cell.x, // z height along y1 at cell.x
z_cxyd = z_cxy1 - z_cxy0; // z height difference along cell.x from y0 to y1
float z_cxym = z_cxyd * RECIPROCAL(MESH_Y_DIST); // z slope per y along cx from y0 to y1 (changes for each cx in cell)
float z_cxym = z_cxyd * RECIPROCAL(MESH_Y_DIST); // z slope per y along cell.x from pos.y to y1 (changes for each cell.x 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 * cell.y; // interpolated mesh z height along cell.x at cell.y (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
// and the z_cxym slope will change, both as a function of cell.x 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) * RECIPROCAL(MESH_Y_DIST) * diff[X_AXIS]; // per-segment adjustment to z_cxym
const float z_sxy0 = z_xmy0 * diff.x, // per-segment adjustment to z_cxy0
z_sxym = (z_xmy1 - z_xmy0) * RECIPROCAL(MESH_Y_DIST) * diff.x; // per-segment adjustment to z_cxym
for (;;) { // for all segments within this mesh cell
if (--segments == 0) COPY(raw, destination); // if this is last segment, use destination for exact
if (--segments == 0) raw = destination; // if this is last segment, use destination for exact
const float z_cxcy = (z_cxy0 + z_cxym * cy) // interpolated mesh z height along cx at cy
const float z_cxcy = (z_cxy0 + z_cxym * cell.y) // interpolated mesh z height along cell.x at cell.y
#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;
planner.buffer_line(raw, scaled_fr_mm_s, active_extruder, segment_xyz_mm
planner.buffer_line(raw.x, raw.y, raw.z + z_cxcy, raw.e, scaled_fr_mm_s, active_extruder, segment_xyz_mm
#if ENABLED(SCARA_FEEDRATE_SCALING)
, inv_duration
#endif
);
raw[Z_AXIS] = z;
if (segments == 0) // done with last segment
return false; // did not set_current_from_destination()
return false; // didn't set current from destination
LOOP_XYZE(i) raw[i] += diff[i];
raw += diff;
cell += diff;
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(cell.x, 0, MESH_X_DIST) || !WITHIN(cell.y, 0, MESH_Y_DIST)) // done within this cell, break to next
break;
// Next segment still within same mesh cell, adjust the per-segment

6
Marlin/src/feature/dac/dac_mcp4728.cpp
View File

@ -36,7 +36,7 @@
#include "dac_mcp4728.h"
uint16_t mcp4728_values[XYZE];
xyze_uint_t mcp4728_values;
/**
* Begin I2C, get current values (input register and eeprom) of mcp4728
@ -121,8 +121,8 @@ uint8_t mcp4728_getDrvPct(const uint8_t channel) { return uint8_t(100.0 * mcp472
* Receives all Drive strengths as 0-100 percent values, updates
* DAC Values array and calls fastwrite to update the DAC.
*/
void mcp4728_setDrvPct(uint8_t pct[XYZE]) {
LOOP_XYZE(i) mcp4728_values[i] = 0.01 * pct[i] * (DAC_STEPPER_MAX);
void mcp4728_setDrvPct(xyze_uint8_t &pct) {
mcp4728_values *= 0.01 * pct * (DAC_STEPPER_MAX);
mcp4728_fastWrite();
}

4
Marlin/src/feature/dac/dac_mcp4728.h
View File

@ -25,6 +25,8 @@
* Arduino library for MicroChip MCP4728 I2C D/A converter.
*/
#include "../../core/types.h"
#include <Wire.h>
#define defaultVDD DAC_STEPPER_MAX //was 5000 but differs with internal Vref
@ -54,4 +56,4 @@ uint16_t mcp4728_getValue(const uint8_t channel);
uint8_t mcp4728_fastWrite();
uint8_t mcp4728_simpleCommand(const byte simpleCommand);
uint8_t mcp4728_getDrvPct(const uint8_t channel);
void mcp4728_setDrvPct(uint8_t pct[XYZE]);
void mcp4728_setDrvPct(xyze_uint8_t &pct);

8
Marlin/src/feature/dac/stepper_dac.cpp
View File

@ -31,8 +31,8 @@
#include "stepper_dac.h"
bool dac_present = false;
const uint8_t dac_order[NUM_AXIS] = DAC_STEPPER_ORDER;
uint8_t dac_channel_pct[XYZE] = DAC_MOTOR_CURRENT_DEFAULT;
constexpr xyze_uint8_t dac_order = DAC_STEPPER_ORDER;
xyze_uint8_t dac_channel_pct = DAC_MOTOR_CURRENT_DEFAULT;
int dac_init() {
#if PIN_EXISTS(DAC_DISABLE)
@ -77,8 +77,8 @@ void dac_current_raw(uint8_t channel, uint16_t val) {
static float dac_perc(int8_t n) { return 100.0 * mcp4728_getValue(dac_order[n]) * RECIPROCAL(DAC_STEPPER_MAX); }
static float dac_amps(int8_t n) { return mcp4728_getDrvPct(dac_order[n]) * (DAC_STEPPER_MAX) * 0.125 * RECIPROCAL(DAC_STEPPER_SENSE); }
uint8_t dac_current_get_percent(AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
void dac_current_set_percents(const uint8_t pct[XYZE]) {
uint8_t dac_current_get_percent(const AxisEnum axis) { return mcp4728_getDrvPct(dac_order[axis]); }
void dac_current_set_percents(xyze_uint8_t &pct) {
LOOP_XYZE(i) dac_channel_pct[i] = pct[dac_order[i]];
mcp4728_setDrvPct(dac_channel_pct);
}

2
Marlin/src/feature/dac/stepper_dac.h
View File

@ -33,4 +33,4 @@ void dac_current_raw(uint8_t channel, uint16_t val);
void dac_print_values();
void dac_commit_eeprom();
uint8_t dac_current_get_percent(AxisEnum axis);
void dac_current_set_percents(const uint8_t pct[XYZE]);
void dac_current_set_percents(xyze_uint8_t &pct);

14
Marlin/src/feature/fwretract.cpp
View File

@ -123,8 +123,8 @@ void FWRetract::retract(const bool retracting
SERIAL_ECHOLNPAIR("retracted_swap[", i, "] ", retracted_swap[i]);
#endif
}
SERIAL_ECHOLNPAIR("current_position[z] ", current_position[Z_AXIS]);
SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]);
SERIAL_ECHOLNPAIR("current_position.z ", current_position.z);
SERIAL_ECHOLNPAIR("current_position.e ", current_position.e);
SERIAL_ECHOLNPAIR("current_hop ", current_hop);
//*/
@ -136,7 +136,7 @@ void FWRetract::retract(const bool retracting
);
// The current position will be the destination for E and Z moves
set_destination_from_current();
destination = current_position;
#if ENABLED(RETRACT_SYNC_MIXING)
const uint8_t old_mixing_tool = mixer.get_current_vtool();
@ -147,7 +147,7 @@ void FWRetract::retract(const bool retracting
if (retracting) {
// Retract by moving from a faux E position back to the current E position
current_retract[active_extruder] = base_retract;
prepare_internal_move_to_destination( // set_current_to_destination
prepare_internal_move_to_destination( // set current to destination
settings.retract_feedrate_mm_s
#if ENABLED(RETRACT_SYNC_MIXING)
* (MIXING_STEPPERS)
@ -171,7 +171,7 @@ void FWRetract::retract(const bool retracting
const float extra_recover = swapping ? settings.swap_retract_recover_extra : settings.retract_recover_extra;
if (extra_recover) {
current_position[E_AXIS] -= extra_recover; // Adjust the current E position by the extra amount to recover
current_position.e -= extra_recover; // Adjust the current E position by the extra amount to recover
sync_plan_position_e(); // Sync the planner position so the extra amount is recovered
}
@ -207,8 +207,8 @@ void FWRetract::retract(const bool retracting
SERIAL_ECHOLNPAIR("retracted_swap[", i, "] ", retracted_swap[i]);
#endif
}
SERIAL_ECHOLNPAIR("current_position[z] ", current_position[Z_AXIS]);
SERIAL_ECHOLNPAIR("current_position[e] ", current_position[E_AXIS]);
SERIAL_ECHOLNPAIR("current_position.z ", current_position.z);
SERIAL_ECHOLNPAIR("current_position.e ", current_position.e);
SERIAL_ECHOLNPAIR("current_hop ", current_hop);
//*/
}

29
Marlin/src/feature/joystick.cpp
View File

@ -71,35 +71,35 @@ Joystick joystick;
#if HAS_JOY_ADC_X || HAS_JOY_ADC_Y || HAS_JOY_ADC_Z
void Joystick::calculate(float (&norm_jog)[XYZ]) {
void Joystick::calculate(xyz_float_t &norm_jog) {
// Do nothing if enable pin (active-low) is not LOW
#if HAS_JOY_ADC_EN
if (READ(JOY_EN_PIN)) return;
#endif
auto _normalize_joy = [](float &norm_jog, const int16_t raw, const int16_t (&joy_limits)[4]) {
auto _normalize_joy = [](float &axis_jog, const int16_t raw, const int16_t (&joy_limits)[4]) {
if (WITHIN(raw, joy_limits[0], joy_limits[3])) {
// within limits, check deadzone
if (raw > joy_limits[2])
norm_jog = (raw - joy_limits[2]) / float(joy_limits[3] - joy_limits[2]);
axis_jog = (raw - joy_limits[2]) / float(joy_limits[3] - joy_limits[2]);
else if (raw < joy_limits[1])
norm_jog = (raw - joy_limits[1]) / float(joy_limits[1] - joy_limits[0]); // negative value
axis_jog = (raw - joy_limits[1]) / float(joy_limits[1] - joy_limits[0]); // negative value
// Map normal to jog value via quadratic relationship
norm_jog = SIGN(norm_jog) * sq(norm_jog);
axis_jog = SIGN(axis_jog) * sq(axis_jog);
}
};
#if HAS_JOY_ADC_X
static constexpr int16_t joy_x_limits[4] = JOY_X_LIMITS;
_normalize_joy(norm_jog[X_AXIS], x.raw, joy_x_limits);
_normalize_joy(norm_jog.x, x.raw, joy_x_limits);
#endif
#if HAS_JOY_ADC_Y
static constexpr int16_t joy_y_limits[4] = JOY_Y_LIMITS;
_normalize_joy(norm_jog[Y_AXIS], y.raw, joy_y_limits);
_normalize_joy(norm_jog.y, y.raw, joy_y_limits);
#endif
#if HAS_JOY_ADC_Z
static constexpr int16_t joy_z_limits[4] = JOY_Z_LIMITS;
_normalize_joy(norm_jog[Z_AXIS], z.raw, joy_z_limits);
_normalize_joy(norm_jog.z, z.raw, joy_z_limits);
#endif
}
@ -129,7 +129,7 @@ Joystick joystick;
// Normalized jog values are 0 for no movement and -1 or +1 for as max feedrate (nonlinear relationship)
// Jog are initialized to zero and handling input can update values but doesn't have to
// You could use a two-axis joystick and a one-axis keypad and they might work together
float norm_jog[XYZ] = { 0 };
xyz_float_t norm_jog{0};
// Use ADC values and defined limits. The active zone is normalized: -1..0 (dead) 0..1
#if HAS_JOY_ADC_X || HAS_JOY_ADC_Y || HAS_JOY_ADC_Z
@ -143,16 +143,13 @@ Joystick joystick;
ExtUI::_joystick_update(norm_jog);
#endif
#if EITHER(ULTIPANEL, EXTENSIBLE_UI)
constexpr float manual_feedrate[XYZE] = MANUAL_FEEDRATE;
#endif
// norm_jog values of [-1 .. 1] maps linearly to [-feedrate .. feedrate]
float move_dist[XYZ] = { 0 }, hypot2 = 0;
xyz_float_t move_dist{0};
float hypot2 = 0;
LOOP_XYZ(i) if (norm_jog[i]) {
move_dist[i] = seg_time * norm_jog[i] *
#if EITHER(ULTIPANEL, EXTENSIBLE_UI)
MMM_TO_MMS(manual_feedrate[i]);
MMM_TO_MMS(manual_feedrate_mm_m[i]);
#else
planner.settings.max_feedrate_mm_s[i];
#endif
@ -160,7 +157,7 @@ Joystick joystick;
}
if (!UNEAR_ZERO(hypot2)) {
LOOP_XYZ(i) current_position[i] += move_dist[i];
current_position += move_dist;
const float length = sqrt(hypot2);
injecting_now = true;
planner.buffer_line(current_position, length / seg_time, active_extruder, length);

4
Marlin/src/feature/joystick.h
View File

@ -25,6 +25,8 @@
* joystick.h - joystick input / jogging
*/
#include "../inc/MarlinConfigPre.h"
#include "../core/types.h"
#include "../core/macros.h"
#include "../module/temperature.h"
@ -46,7 +48,7 @@ class Joystick {
#if ENABLED(JOYSTICK_DEBUG)
static void report();
#endif
static void calculate(float (&norm_jog)[XYZ]);
static void calculate(xyz_float_t &norm_jog);
static void inject_jog_moves();
};

22
Marlin/src/feature/pause.cpp
View File

@ -64,7 +64,7 @@
// private:
static float resume_position[XYZE];
static xyze_pos_t resume_position;
PauseMode pause_mode = PAUSE_MODE_PAUSE_PRINT;
@ -126,8 +126,8 @@ void do_pause_e_move(const float &length, const feedRate_t &fr_mm_s) {
#if HAS_FILAMENT_SENSOR
runout.reset();
#endif
current_position[E_AXIS] += length / planner.e_factor[active_extruder];
planner.buffer_line(current_position, fr_mm_s, active_extruder);
current_position.e += length / planner.e_factor[active_extruder];
line_to_current_position(fr_mm_s);
planner.synchronize();
}
@ -385,7 +385,7 @@ bool unload_filament(const float &unload_length, const bool show_lcd/*=false*/,
*/
uint8_t did_pause_print = 0;
bool pause_print(const float &retract, const point_t &park_point, const float &unload_length/*=0*/, const bool show_lcd/*=false*/ DXC_ARGS) {
bool pause_print(const float &retract, const xyz_pos_t &park_point, const float &unload_length/*=0*/, const bool show_lcd/*=false*/ DXC_ARGS) {
#if !HAS_LCD_MENU
UNUSED(show_lcd);
@ -432,7 +432,7 @@ bool pause_print(const float &retract, const point_t &park_point, const float &u
print_job_timer.pause();
// Save current position
COPY(resume_position, current_position);
resume_position = current_position;
// Wait for buffered blocks to complete
planner.synchronize();
@ -611,10 +611,10 @@ void wait_for_confirmation(const bool is_reload/*=false*/, const int8_t max_beep
* - Display "wait for print to resume"
* - Re-prime the nozzle...
* - FWRETRACT: Recover/prime from the prior G10.
* - !FWRETRACT: Retract by resume_position[E], if negative.
* - !FWRETRACT: Retract by resume_position.e, if negative.
* Not sure how this logic comes into use.
* - Move the nozzle back to resume_position
* - Sync the planner E to resume_position[E]
* - Sync the planner E to resume_position.e
* - Send host action for resume, if configured
* - Resume the current SD print job, if any
*/
@ -652,13 +652,13 @@ void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_le
#endif
// If resume_position is negative
if (resume_position[E_AXIS] < 0) do_pause_e_move(resume_position[E_AXIS], feedRate_t(PAUSE_PARK_RETRACT_FEEDRATE));
if (resume_position.e < 0) do_pause_e_move(resume_position.e, feedRate_t(PAUSE_PARK_RETRACT_FEEDRATE));
// Move XY to starting position, then Z
do_blocking_move_to_xy(resume_position[X_AXIS], resume_position[Y_AXIS], feedRate_t(NOZZLE_PARK_XY_FEEDRATE));
do_blocking_move_to_xy(xy_pos_t(resume_position), feedRate_t(NOZZLE_PARK_XY_FEEDRATE));
// Move Z_AXIS to saved position
do_blocking_move_to_z(resume_position[Z_AXIS], feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
do_blocking_move_to_z(resume_position.z, feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
#if ADVANCED_PAUSE_RESUME_PRIME != 0
do_pause_e_move(ADVANCED_PAUSE_RESUME_PRIME, feedRate_t(ADVANCED_PAUSE_PURGE_FEEDRATE));
@ -666,7 +666,7 @@ void resume_print(const float &slow_load_length/*=0*/, const float &fast_load_le
// Now all extrusion positions are resumed and ready to be confirmed
// Set extruder to saved position
planner.set_e_position_mm((destination[E_AXIS] = current_position[E_AXIS] = resume_position[E_AXIS]));
planner.set_e_position_mm((destination.e = current_position.e = resume_position.e));
#if HAS_LCD_MENU
lcd_pause_show_message(PAUSE_MESSAGE_STATUS);

2
Marlin/src/feature/pause.h
View File

@ -83,7 +83,7 @@ extern uint8_t did_pause_print;
void do_pause_e_move(const float &length, const feedRate_t &fr_mm_s);
bool pause_print(const float &retract, const point_t &park_point, const float &unload_length=0, const bool show_lcd=false DXC_PARAMS);
bool pause_print(const float &retract, const xyz_pos_t &park_point, const float &unload_length=0, const bool show_lcd=false DXC_PARAMS);
void wait_for_confirmation(const bool is_reload=false, const int8_t max_beep_count=0 DXC_PARAMS);

32
Marlin/src/feature/power_loss_recovery.cpp
View File

@ -156,7 +156,7 @@ void PrintJobRecovery::save(const bool force/*=false*/, const bool save_queue/*=
|| ELAPSED(ms, next_save_ms)
#endif
// Save if Z is above the last-saved position by some minimum height
|| current_position[Z_AXIS] > info.current_position[Z_AXIS] + POWER_LOSS_MIN_Z_CHANGE
|| current_position.z > info.current_position.z + POWER_LOSS_MIN_Z_CHANGE
#endif
) {
@ -170,12 +170,12 @@ void PrintJobRecovery::save(const bool force/*=false*/, const bool save_queue/*=
info.valid_foot = info.valid_head;
// Machine state
COPY(info.current_position, current_position);
info.current_position = current_position;
#if HAS_HOME_OFFSET
COPY(info.home_offset, home_offset);
info.home_offset = home_offset;
#endif
#if HAS_POSITION_SHIFT
COPY(info.position_shift, position_shift);
info.position_shift = position_shift;
#endif
info.feedrate = uint16_t(feedrate_mm_s * 60.0f);
@ -361,13 +361,13 @@ void PrintJobRecovery::resume() {
// Move back to the saved XY
sprintf_P(cmd, PSTR("G1 X%s Y%s F3000"),
dtostrf(info.current_position[X_AXIS], 1, 3, str_1),
dtostrf(info.current_position[Y_AXIS], 1, 3, str_2)
dtostrf(info.current_position.x, 1, 3, str_1),
dtostrf(info.current_position.y, 1, 3, str_2)
);
gcode.process_subcommands_now(cmd);
// Move back to the saved Z
dtostrf(info.current_position[Z_AXIS], 1, 3, str_1);
dtostrf(info.current_position.z, 1, 3, str_1);
#if Z_HOME_DIR > 0
sprintf_P(cmd, PSTR("G1 Z%s F200"), str_1);
#else
@ -388,22 +388,20 @@ void PrintJobRecovery::resume() {
gcode.process_subcommands_now(cmd);
// Restore E position with G92.9
sprintf_P(cmd, PSTR("G92.9 E%s"), dtostrf(info.current_position[E_AXIS], 1, 3, str_1));
sprintf_P(cmd, PSTR("G92.9 E%s"), dtostrf(info.current_position.e, 1, 3, str_1));
gcode.process_subcommands_now(cmd);
// Relative axis modes
gcode.axis_relative = info.axis_relative;
#if HAS_HOME_OFFSET
home_offset = info.home_offset;
#endif
#if HAS_POSITION_SHIFT
position_shift = info.position_shift;
#endif
#if HAS_HOME_OFFSET || HAS_POSITION_SHIFT
LOOP_XYZ(i) {
#if HAS_HOME_OFFSET
home_offset[i] = info.home_offset[i];
#endif
#if HAS_POSITION_SHIFT
position_shift[i] = info.position_shift[i];
#endif
update_workspace_offset((AxisEnum)i);
}
LOOP_XYZ(i) update_workspace_offset((AxisEnum)i);
#endif
// Resume the SD file from the last position

6
Marlin/src/feature/power_loss_recovery.h
View File

@ -44,13 +44,13 @@ typedef struct {
uint8_t valid_head;
// Machine state
float current_position[NUM_AXIS];
xyze_pos_t current_position;
#if HAS_HOME_OFFSET
float home_offset[XYZ];
xyz_pos_t home_offset;
#endif
#if HAS_POSITION_SHIFT
float position_shift[XYZ];
xyz_pos_t position_shift;
#endif
uint16_t feedrate;

16
Marlin/src/feature/prusa_MMU2/mmu2.cpp
View File

@ -550,10 +550,10 @@ bool MMU2::get_response() {
*/
void MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) {
constexpr xyz_pos_t park_point = NOZZLE_PARK_POINT;
bool response = false;
mmu_print_saved = false;
point_t park_point = NOZZLE_PARK_POINT;
float resume_position[XYZE];
xyz_pos_t resume_position;
int16_t resume_hotend_temp;
KEEPALIVE_STATE(PAUSED_FOR_USER);
@ -572,7 +572,7 @@ void MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) {
SERIAL_ECHOLNPGM("MMU not responding");
resume_hotend_temp = thermalManager.degTargetHotend(active_extruder);
COPY(resume_position, current_position);
resume_position = current_position;
if (move_axes && all_axes_homed())
nozzle.park(2, park_point /*= NOZZLE_PARK_POINT*/);
@ -604,10 +604,10 @@ void MMU2::manage_response(const bool move_axes, const bool turn_off_nozzle) {
BUZZ(200, 404);
// Move XY to starting position, then Z
do_blocking_move_to_xy(resume_position[X_AXIS], resume_position[Y_AXIS], feedRate_t(NOZZLE_PARK_XY_FEEDRATE));
do_blocking_move_to_xy(resume_position, feedRate_t(NOZZLE_PARK_XY_FEEDRATE));
// Move Z_AXIS to saved position
do_blocking_move_to_z(resume_position[Z_AXIS], feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
do_blocking_move_to_z(resume_position.z, feedRate_t(NOZZLE_PARK_Z_FEEDRATE));
}
else {
BUZZ(200, 404);
@ -698,8 +698,8 @@ void MMU2::filament_runout() {
LCD_MESSAGEPGM(MSG_MMU2_EJECTING_FILAMENT);
enable_E0();
current_position[E_AXIS] -= MMU2_FILAMENTCHANGE_EJECT_FEED;
planner.buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], 2500 / 60, active_extruder);
current_position.e -= MMU2_FILAMENTCHANGE_EJECT_FEED;
line_to_current_position(2500 / 60);
planner.synchronize();
command(MMU_CMD_E0 + index);
manage_response(false, false);
@ -787,7 +787,7 @@ void MMU2::filament_runout() {
DEBUG_ECHO_START();
DEBUG_ECHOLNPAIR("E step ", es, "/", fr_mm_m);
current_position[E_AXIS] += es;
current_position.e += es;
line_to_current_position(MMM_TO_MMS(fr_mm_m));
planner.synchronize();

4
Marlin/src/feature/runout.h
View File

@ -327,14 +327,14 @@ class FilamentSensorBase {
}
static inline void block_completed(const block_t* const b) {
if (b->steps[X_AXIS] || b->steps[Y_AXIS] || b->steps[Z_AXIS]
if (b->steps.x || b->steps.y || b->steps.z
#if ENABLED(ADVANCED_PAUSE_FEATURE)
|| did_pause_print // Allow pause purge move to re-trigger runout state
#endif
) {
// Only trigger on extrusion with XYZ movement to allow filament change and retract/recover.
const uint8_t e = b->extruder;
const int32_t steps = b->steps[E_AXIS];
const int32_t steps = b->steps.e;
runout_mm_countdown[e] -= (TEST(b->direction_bits, E_AXIS) ? -steps : steps) * planner.steps_to_mm[E_AXIS_N(e)];
}
}

4
Marlin/src/feature/tmc_util.h
View File

@ -367,9 +367,9 @@ void test_tmc_connection(const bool test_x, const bool test_y, const bool test_z
constexpr uint16_t default_sg_guard_duration = 400;
struct slow_homing_t {
struct { uint32_t x, y; } acceleration;
xy_ulong_t acceleration;
#if HAS_CLASSIC_JERK
struct { float x, y; } jerk;
xy_float_t jerk_xy;
#endif
};
#endif