From a4ed988c60f5eb030418a5d82f8905c7be537ee9 Mon Sep 17 00:00:00 2001
From: Scott Lahteine <sourcetree@thinkyhead.com>
Date: Sat, 21 May 2016 17:38:14 -0700
Subject: [PATCH] Two index finding functions for MBL
---
Marlin/Marlin_main.cpp | 85 +++++++++++++++++-----------------
Marlin/configuration_store.cpp | 10 ++--
Marlin/mesh_bed_leveling.h | 68 ++++++++++++++++-----------
Marlin/ultralcd.cpp | 6 +--
4 files changed, 90 insertions(+), 79 deletions(-)
diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp
index c0884cd23a..736de5ac97 100644
--- a/Marlin/Marlin_main.cpp
+++ b/Marlin/Marlin_main.cpp
@@ -3008,7 +3008,7 @@ inline void gcode_G28() {
return;
}
- int8_t ix, iy;
+ int8_t px, py;
float z;
switch (state) {
@@ -3023,10 +3023,10 @@ inline void gcode_G28() {
SERIAL_PROTOCOLPGM("\nZ offset: ");
SERIAL_PROTOCOL_F(mbl.z_offset, 5);
SERIAL_PROTOCOLLNPGM("\nMeasured points:");
- for (int y = 0; y < MESH_NUM_Y_POINTS; y++) {
- for (int x = 0; x < MESH_NUM_X_POINTS; x++) {
+ for (py = 0; py < MESH_NUM_Y_POINTS; py++) {
+ for (px = 0; px < MESH_NUM_X_POINTS; px++) {
SERIAL_PROTOCOLPGM(" ");
- SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
+ SERIAL_PROTOCOL_F(mbl.z_values[py][px], 5);
}
SERIAL_EOL;
}
@@ -3058,8 +3058,8 @@ inline void gcode_G28() {
}
// If there's another point to sample, move there with optional lift.
if (probe_point < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
- mbl.zigzag(probe_point, ix, iy);
- _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy));
+ mbl.zigzag(probe_point, px, py);
+ _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py));
probe_point++;
}
else {
@@ -3082,8 +3082,8 @@ inline void gcode_G28() {
case MeshSet:
if (code_seen('X')) {
- ix = code_value_long() - 1;
- if (ix < 0 || ix >= MESH_NUM_X_POINTS) {
+ px = code_value_long() - 1;
+ if (px < 0 || px >= MESH_NUM_X_POINTS) {
SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n");
return;
}
@@ -3093,8 +3093,8 @@ inline void gcode_G28() {
return;
}
if (code_seen('Y')) {
- iy = code_value_long() - 1;
- if (iy < 0 || iy >= MESH_NUM_Y_POINTS) {
+ py = code_value_long() - 1;
+ if (py < 0 || py >= MESH_NUM_Y_POINTS) {
SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n");
return;
}
@@ -3110,7 +3110,7 @@ inline void gcode_G28() {
SERIAL_PROTOCOLPGM("Z not entered.\n");
return;
}
- mbl.z_values[iy][ix] = z;
+ mbl.z_values[py][px] = z;
break;
case MeshSetZOffset:
@@ -5905,36 +5905,35 @@ inline void gcode_M410() { stepper.quick_stop(); }
*/
inline void gcode_M421() {
float x = 0, y = 0, z = 0;
- int8_t i = 0, j = 0;
+ int8_t px = 0, py = 0;
bool err = false, hasX, hasY, hasZ, hasI, hasJ;
if ((hasX = code_seen('X'))) x = code_value();
if ((hasY = code_seen('Y'))) y = code_value();
- if ((hasI = code_seen('I'))) i = code_value();
- if ((hasJ = code_seen('J'))) j = code_value();
if ((hasZ = code_seen('Z'))) z = code_value();
+ if ((hasI = code_seen('I'))) px = code_value();
+ if ((hasJ = code_seen('J'))) py = code_value();
if (hasX && hasY && hasZ) {
- int8_t ix = mbl.select_x_index(x),
- iy = mbl.select_y_index(y);
+ px = mbl.probe_index_x(x);
+ py = mbl.probe_index_y(y);
- if (ix >= 0 && iy >= 0)
- mbl.set_z(ix, iy, z);
+ if (px >= 0 && py >= 0)
+ mbl.set_z(px, py, z);
else {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
}
}
else if (hasI && hasJ && hasZ) {
- if (i >= 0 && i < MESH_NUM_X_POINTS && j >= 0 && j < MESH_NUM_Y_POINTS)
- mbl.set_z(i, j, z);
+ if (px >= 0 && px < MESH_NUM_X_POINTS && py >= 0 && py < MESH_NUM_Y_POINTS)
+ mbl.set_z(px, py, z);
else {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
}
}
- else
- {
+ else {
SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_M421_REQUIRES_XYZ);
}
@@ -7303,52 +7302,52 @@ void mesh_buffer_line(float x, float y, float z, const float e, float feed_rate,
set_current_to_destination();
return;
}
- int pix = mbl.select_x_index(current_position[X_AXIS] - home_offset[X_AXIS]);
- int piy = mbl.select_y_index(current_position[Y_AXIS] - home_offset[Y_AXIS]);
- int ix = mbl.select_x_index(x - home_offset[X_AXIS]);
- int iy = mbl.select_y_index(y - home_offset[Y_AXIS]);
- pix = min(pix, MESH_NUM_X_POINTS - 2);
- piy = min(piy, MESH_NUM_Y_POINTS - 2);
- ix = min(ix, MESH_NUM_X_POINTS - 2);
- iy = min(iy, MESH_NUM_Y_POINTS - 2);
- if (pix == ix && piy == iy) {
+ int pcx = mbl.cel_index_x(current_position[X_AXIS] - home_offset[X_AXIS]);
+ int pcy = mbl.cel_index_y(current_position[Y_AXIS] - home_offset[Y_AXIS]);
+ int cx = mbl.cel_index_x(x - home_offset[X_AXIS]);
+ int cy = mbl.cel_index_y(y - home_offset[Y_AXIS]);
+ NOMORE(pcx, MESH_NUM_X_POINTS - 2);
+ NOMORE(pcy, MESH_NUM_Y_POINTS - 2);
+ NOMORE(cx, MESH_NUM_X_POINTS - 2);
+ NOMORE(cy, MESH_NUM_Y_POINTS - 2);
+ if (pcx == cx && pcy == cy) {
// Start and end on same mesh square
planner.buffer_line(x, y, z, e, feed_rate, extruder);
set_current_to_destination();
return;
}
float nx, ny, nz, ne, normalized_dist;
- if (ix > pix && TEST(x_splits, ix)) {
- nx = mbl.get_x(ix) + home_offset[X_AXIS];
+ if (cx > pcx && TEST(x_splits, cx)) {
+ nx = mbl.get_probe_x(cx) + home_offset[X_AXIS];
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
- CBI(x_splits, ix);
+ CBI(x_splits, cx);
}
- else if (ix < pix && TEST(x_splits, pix)) {
- nx = mbl.get_x(pix) + home_offset[X_AXIS];
+ else if (cx < pcx && TEST(x_splits, pcx)) {
+ nx = mbl.get_probe_x(pcx) + home_offset[X_AXIS];
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[X_AXIS]);
ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
- CBI(x_splits, pix);
+ CBI(x_splits, pcx);
}
- else if (iy > piy && TEST(y_splits, iy)) {
- ny = mbl.get_y(iy) + home_offset[Y_AXIS];
+ else if (cy > pcy && TEST(y_splits, cy)) {
+ ny = mbl.get_probe_y(cy) + home_offset[Y_AXIS];
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
- CBI(y_splits, iy);
+ CBI(y_splits, cy);
}
- else if (iy < piy && TEST(y_splits, piy)) {
- ny = mbl.get_y(piy) + home_offset[Y_AXIS];
+ else if (cy < pcy && TEST(y_splits, pcy)) {
+ ny = mbl.get_probe_y(pcy) + home_offset[Y_AXIS];
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[Y_AXIS]);
nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_AXIS]) * normalized_dist;
ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
- CBI(y_splits, piy);
+ CBI(y_splits, pcy);
}
else {
// Already split on a border
diff --git a/Marlin/configuration_store.cpp b/Marlin/configuration_store.cpp
index 8fed0a93e4..66fde12d32 100644
--- a/Marlin/configuration_store.cpp
+++ b/Marlin/configuration_store.cpp
@@ -733,13 +733,13 @@ void Config_PrintSettings(bool forReplay) {
SERIAL_ECHOPAIR(" X", MESH_NUM_X_POINTS);
SERIAL_ECHOPAIR(" Y", MESH_NUM_Y_POINTS);
SERIAL_EOL;
- for (uint8_t y = 0; y < MESH_NUM_Y_POINTS; y++) {
- for (uint8_t x = 0; x < MESH_NUM_X_POINTS; x++) {
+ for (uint8_t py = 0; py < MESH_NUM_Y_POINTS; py++) {
+ for (uint8_t px = 0; px < MESH_NUM_X_POINTS; px++) {
CONFIG_ECHO_START;
- SERIAL_ECHOPAIR(" M421 X", mbl.get_x(x));
- SERIAL_ECHOPAIR(" Y", mbl.get_y(y));
+ SERIAL_ECHOPAIR(" M421 X", mbl.get_probe_x(px));
+ SERIAL_ECHOPAIR(" Y", mbl.get_probe_y(py));
SERIAL_ECHOPGM(" Z");
- SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5);
+ SERIAL_PROTOCOL_F(mbl.z_values[py][px], 5);
SERIAL_EOL;
}
}
diff --git a/Marlin/mesh_bed_leveling.h b/Marlin/mesh_bed_leveling.h
index f51501a220..248a36aa4c 100644
--- a/Marlin/mesh_bed_leveling.h
+++ b/Marlin/mesh_bed_leveling.h
@@ -37,33 +37,45 @@
void reset();
- static FORCE_INLINE float get_x(int8_t i) { return MESH_MIN_X + (MESH_X_DIST) * i; }
- static FORCE_INLINE float get_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
- void set_z(int8_t ix, int8_t iy, float z) { z_values[iy][ix] = z; }
-
- inline void zigzag(int8_t index, int8_t &ix, int8_t &iy) {
- ix = index % (MESH_NUM_X_POINTS);
- iy = index / (MESH_NUM_X_POINTS);
- if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag
+ static FORCE_INLINE float get_probe_x(int8_t i) { return MESH_MIN_X + (MESH_X_DIST) * i; }
+ static FORCE_INLINE float get_probe_y(int8_t i) { return MESH_MIN_Y + (MESH_Y_DIST) * i; }
+ void set_z(int8_t px, int8_t py, float z) { z_values[py][px] = z; }
+
+ inline void zigzag(int8_t index, int8_t &px, int8_t &py) {
+ px = index % (MESH_NUM_X_POINTS);
+ py = index / (MESH_NUM_X_POINTS);
+ if (py & 1) px = (MESH_NUM_X_POINTS - 1) - px; // Zig zag
}
void set_zigzag_z(int8_t index, float z) {
- int8_t ix, iy;
- zigzag(index, ix, iy);
- set_z(ix, iy, z);
+ int8_t px, py;
+ zigzag(index, px, py);
+ set_z(px, py, z);
+ }
+
+ int8_t cel_index_x(float x) {
+ int8_t cx = 1;
+ while (x > get_probe_x(cx) && cx < MESH_NUM_X_POINTS - 1) cx++; // For 3x3 range is 1 to 2
+ return cx - 1; // so this will return 0 - 1
+ }
+
+ int8_t cel_index_y(float y) {
+ int8_t cy = 1;
+ while (y > get_probe_y(cy) && cy < MESH_NUM_Y_POINTS - 1) cy++;
+ return cy - 1;
}
- int8_t select_x_index(float x) {
- for (uint8_t i = MESH_NUM_X_POINTS; i--;)
- if (fabs(x - get_x(i)) <= (MESH_X_DIST) / 2)
- return i;
+ int8_t probe_index_x(float x) {
+ for (int8_t px = MESH_NUM_X_POINTS; px--;)
+ if (fabs(x - get_probe_x(px)) <= (MESH_X_DIST) / 2)
+ return px;
return -1;
}
- int8_t select_y_index(float y) {
- for (uint8_t i = MESH_NUM_Y_POINTS; i--;)
- if (fabs(y - get_y(i)) <= (MESH_Y_DIST) / 2)
- return i;
+ int8_t probe_index_y(float y) {
+ for (int8_t py = MESH_NUM_Y_POINTS; py--;)
+ if (fabs(y - get_probe_y(py)) <= (MESH_Y_DIST) / 2)
+ return py;
return -1;
}
@@ -74,18 +86,18 @@
}
float get_z(float x0, float y0) {
- int8_t x_index = select_x_index(x0);
- int8_t y_index = select_y_index(y0);
- if (x_index < 0 || y_index < 0) return z_offset;
+ int8_t cx = cel_index_x(x0),
+ cy = cel_index_y(y0);
+ if (cx < 0 || cy < 0) return z_offset;
float z1 = calc_z0(x0,
- get_x(x_index), z_values[y_index][x_index],
- get_x(x_index + 1), z_values[y_index][x_index + 1]);
+ get_probe_x(cx), z_values[cy][cx],
+ get_probe_x(cx + 1), z_values[cy][cx + 1]);
float z2 = calc_z0(x0,
- get_x(x_index), z_values[y_index + 1][x_index],
- get_x(x_index + 1), z_values[y_index + 1][x_index + 1]);
+ get_probe_x(cx), z_values[cy + 1][cx],
+ get_probe_x(cx + 1), z_values[cy + 1][cx + 1]);
float z0 = calc_z0(y0,
- get_y(y_index), z1,
- get_y(y_index + 1), z2);
+ get_probe_y(cy), z1,
+ get_probe_y(cy + 1), z2);
return z0 + z_offset;
}
};
diff --git a/Marlin/ultralcd.cpp b/Marlin/ultralcd.cpp
index dd99b2961f..46ccb19645 100644
--- a/Marlin/ultralcd.cpp
+++ b/Marlin/ultralcd.cpp
@@ -1013,9 +1013,9 @@ void lcd_cooldown() {
lcd_goto_menu(_lcd_level_bed_moving);
// _mbl_goto_xy runs the menu loop until the move is done
- int8_t ix, iy;
- mbl.zigzag(_lcd_level_bed_position, ix, iy);
- _mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy));
+ int8_t px, py;
+ mbl.zigzag(_lcd_level_bed_position, px, py);
+ _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py));
// After the blocking function returns, change menus
lcd_goto_menu(_lcd_level_bed_get_z);
--
GitLab