andrey
/
Marlin_FB4S
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Two index finding functions for MBL

pull/1/head
Scott Lahteine 9 years ago
parent
678cbad76a
commit
a4ed988c60
4 changed files with 90 additions and 79 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 85
      Marlin/Marlin_main.cpp
  2. 10
      Marlin/configuration_store.cpp
  3. 68
      Marlin/mesh_bed_leveling.h
  4. 6
      Marlin/ultralcd.cpp

85
Marlin/Marlin_main.cpp
View File

@ -3008,7 +3008,7 @@ inline void gcode_G28() {
return; return;
} }
int8_t ix, iy; int8_t px, py;
float z; float z;
switch (state) { switch (state) {
@ -3023,10 +3023,10 @@ inline void gcode_G28() {
SERIAL_PROTOCOLPGM("\nZ offset: "); SERIAL_PROTOCOLPGM("\nZ offset: ");
SERIAL_PROTOCOL_F(mbl.z_offset, 5); SERIAL_PROTOCOL_F(mbl.z_offset, 5);
SERIAL_PROTOCOLLNPGM("\nMeasured points:"); SERIAL_PROTOCOLLNPGM("\nMeasured points:");
for (int y = 0; y < MESH_NUM_Y_POINTS; y++) { for (py = 0; py < MESH_NUM_Y_POINTS; py++) {
for (int x = 0; x < MESH_NUM_X_POINTS; x++) { for (px = 0; px < MESH_NUM_X_POINTS; px++) {
SERIAL_PROTOCOLPGM(" "); SERIAL_PROTOCOLPGM(" ");
SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5); SERIAL_PROTOCOL_F(mbl.z_values[py][px], 5);
} }
SERIAL_EOL; SERIAL_EOL;
} }
@ -3058,8 +3058,8 @@ inline void gcode_G28() {
} }
// If there's another point to sample, move there with optional lift. // If there's another point to sample, move there with optional lift.
if (probe_point < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) { if (probe_point < (MESH_NUM_X_POINTS) * (MESH_NUM_Y_POINTS)) {
mbl.zigzag(probe_point, ix, iy); mbl.zigzag(probe_point, px, py);
_mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy)); _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py));
probe_point++; probe_point++;
} }
else { else {
@ -3082,8 +3082,8 @@ inline void gcode_G28() {
case MeshSet: case MeshSet:
if (code_seen('X')) { if (code_seen('X')) {
ix = code_value_long() - 1; px = code_value_long() - 1;
if (ix < 0 || ix >= MESH_NUM_X_POINTS) { if (px < 0 || px >= MESH_NUM_X_POINTS) {
SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n"); SERIAL_PROTOCOLPGM("X out of range (1-" STRINGIFY(MESH_NUM_X_POINTS) ").\n");
return; return;
} }
@ -3093,8 +3093,8 @@ inline void gcode_G28() {
return; return;
} }
if (code_seen('Y')) { if (code_seen('Y')) {
iy = code_value_long() - 1; py = code_value_long() - 1;
if (iy < 0 || iy >= MESH_NUM_Y_POINTS) { if (py < 0 || py >= MESH_NUM_Y_POINTS) {
SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n"); SERIAL_PROTOCOLPGM("Y out of range (1-" STRINGIFY(MESH_NUM_Y_POINTS) ").\n");
return; return;
} }
@ -3110,7 +3110,7 @@ inline void gcode_G28() {
SERIAL_PROTOCOLPGM("Z not entered.\n"); SERIAL_PROTOCOLPGM("Z not entered.\n");
return; return;
} }
mbl.z_values[iy][ix] = z; mbl.z_values[py][px] = z;
break; break;
case MeshSetZOffset: case MeshSetZOffset:
@ -5905,36 +5905,35 @@ inline void gcode_M410() { stepper.quick_stop(); }
*/ */
inline void gcode_M421() { inline void gcode_M421() {
float x = 0, y = 0, z = 0; 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; bool err = false, hasX, hasY, hasZ, hasI, hasJ;
if ((hasX = code_seen('X'))) x = code_value(); if ((hasX = code_seen('X'))) x = code_value();
if ((hasY = code_seen('Y'))) y = 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 ((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) { if (hasX && hasY && hasZ) {
int8_t ix = mbl.select_x_index(x), px = mbl.probe_index_x(x);
iy = mbl.select_y_index(y); py = mbl.probe_index_y(y);
if (ix >= 0 && iy >= 0) if (px >= 0 && py >= 0)
mbl.set_z(ix, iy, z); mbl.set_z(px, py, z);
else { else {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY); SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
} }
} }
else if (hasI && hasJ && hasZ) { else if (hasI && hasJ && hasZ) {
if (i >= 0 && i < MESH_NUM_X_POINTS && j >= 0 && j < MESH_NUM_Y_POINTS) if (px >= 0 && px < MESH_NUM_X_POINTS && py >= 0 && py < MESH_NUM_Y_POINTS)
mbl.set_z(i, j, z); mbl.set_z(px, py, z);
else { else {
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY); SERIAL_ERRORLNPGM(MSG_ERR_MESH_XY);
} }
} }
else else {
{
SERIAL_ERROR_START; SERIAL_ERROR_START;
SERIAL_ERRORLNPGM(MSG_ERR_M421_REQUIRES_XYZ); 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(); set_current_to_destination();
return; return;
} }
int pix = mbl.select_x_index(current_position[X_AXIS] - home_offset[X_AXIS]); int pcx = mbl.cel_index_x(current_position[X_AXIS] - home_offset[X_AXIS]);
int piy = mbl.select_y_index(current_position[Y_AXIS] - home_offset[Y_AXIS]); int pcy = mbl.cel_index_y(current_position[Y_AXIS] - home_offset[Y_AXIS]);
int ix = mbl.select_x_index(x - home_offset[X_AXIS]); int cx = mbl.cel_index_x(x - home_offset[X_AXIS]);
int iy = mbl.select_y_index(y - home_offset[Y_AXIS]); int cy = mbl.cel_index_y(y - home_offset[Y_AXIS]);
pix = min(pix, MESH_NUM_X_POINTS - 2); NOMORE(pcx, MESH_NUM_X_POINTS - 2);
piy = min(piy, MESH_NUM_Y_POINTS - 2); NOMORE(pcy, MESH_NUM_Y_POINTS - 2);
ix = min(ix, MESH_NUM_X_POINTS - 2); NOMORE(cx, MESH_NUM_X_POINTS - 2);
iy = min(iy, MESH_NUM_Y_POINTS - 2); NOMORE(cy, MESH_NUM_Y_POINTS - 2);
if (pix == ix && piy == iy) { if (pcx == cx && pcy == cy) {
// Start and end on same mesh square // Start and end on same mesh square
planner.buffer_line(x, y, z, e, feed_rate, extruder); planner.buffer_line(x, y, z, e, feed_rate, extruder);
set_current_to_destination(); set_current_to_destination();
return; return;
} }
float nx, ny, nz, ne, normalized_dist; float nx, ny, nz, ne, normalized_dist;
if (ix > pix && TEST(x_splits, ix)) { if (cx > pcx && TEST(x_splits, cx)) {
nx = mbl.get_x(ix) + home_offset[X_AXIS]; nx = mbl.get_probe_x(cx) + home_offset[X_AXIS];
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[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; ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_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; 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)) { else if (cx < pcx && TEST(x_splits, pcx)) {
nx = mbl.get_x(pix) + home_offset[X_AXIS]; nx = mbl.get_probe_x(pcx) + home_offset[X_AXIS];
normalized_dist = (nx - current_position[X_AXIS]) / (x - current_position[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; ny = current_position[Y_AXIS] + (y - current_position[Y_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_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; 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)) { else if (cy > pcy && TEST(y_splits, cy)) {
ny = mbl.get_y(iy) + home_offset[Y_AXIS]; ny = mbl.get_probe_y(cy) + home_offset[Y_AXIS];
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[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; nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_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; 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)) { else if (cy < pcy && TEST(y_splits, pcy)) {
ny = mbl.get_y(piy) + home_offset[Y_AXIS]; ny = mbl.get_probe_y(pcy) + home_offset[Y_AXIS];
normalized_dist = (ny - current_position[Y_AXIS]) / (y - current_position[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; nx = current_position[X_AXIS] + (x - current_position[X_AXIS]) * normalized_dist;
nz = current_position[Z_AXIS] + (z - current_position[Z_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; ne = current_position[E_AXIS] + (e - current_position[E_AXIS]) * normalized_dist;
CBI(y_splits, piy); CBI(y_splits, pcy);
} }
else { else {
// Already split on a border // Already split on a border

10
Marlin/configuration_store.cpp
View File

@ -733,13 +733,13 @@ void Config_PrintSettings(bool forReplay) {
SERIAL_ECHOPAIR(" X", MESH_NUM_X_POINTS); SERIAL_ECHOPAIR(" X", MESH_NUM_X_POINTS);
SERIAL_ECHOPAIR(" Y", MESH_NUM_Y_POINTS); SERIAL_ECHOPAIR(" Y", MESH_NUM_Y_POINTS);
SERIAL_EOL; SERIAL_EOL;
for (uint8_t y = 0; y < MESH_NUM_Y_POINTS; y++) { for (uint8_t py = 0; py < MESH_NUM_Y_POINTS; py++) {
for (uint8_t x = 0; x < MESH_NUM_X_POINTS; x++) { for (uint8_t px = 0; px < MESH_NUM_X_POINTS; px++) {
CONFIG_ECHO_START; CONFIG_ECHO_START;
SERIAL_ECHOPAIR(" M421 X", mbl.get_x(x)); SERIAL_ECHOPAIR(" M421 X", mbl.get_probe_x(px));
SERIAL_ECHOPAIR(" Y", mbl.get_y(y)); SERIAL_ECHOPAIR(" Y", mbl.get_probe_y(py));
SERIAL_ECHOPGM(" Z"); SERIAL_ECHOPGM(" Z");
SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5); SERIAL_PROTOCOL_F(mbl.z_values[py][px], 5);
SERIAL_EOL; SERIAL_EOL;
} }
} }

68
Marlin/mesh_bed_leveling.h
View File

@ -37,33 +37,45 @@
void reset(); void reset();
static FORCE_INLINE float get_x(int8_t i) { return MESH_MIN_X + (MESH_X_DIST) * i; } static FORCE_INLINE float get_probe_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; } static FORCE_INLINE float get_probe_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; } void set_z(int8_t px, int8_t py, float z) { z_values[py][px] = z; }
inline void zigzag(int8_t index, int8_t &ix, int8_t &iy) { inline void zigzag(int8_t index, int8_t &px, int8_t &py) {
ix = index % (MESH_NUM_X_POINTS); px = index % (MESH_NUM_X_POINTS);
iy = index / (MESH_NUM_X_POINTS); py = index / (MESH_NUM_X_POINTS);
if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // Zig zag if (py & 1) px = (MESH_NUM_X_POINTS - 1) - px; // Zig zag
} }
void set_zigzag_z(int8_t index, float z) { void set_zigzag_z(int8_t index, float z) {
int8_t ix, iy; int8_t px, py;
zigzag(index, ix, iy); zigzag(index, px, py);
set_z(ix, iy, z); 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) { int8_t probe_index_x(float x) {
for (uint8_t i = MESH_NUM_X_POINTS; i--;) for (int8_t px = MESH_NUM_X_POINTS; px--;)
if (fabs(x - get_x(i)) <= (MESH_X_DIST) / 2) if (fabs(x - get_probe_x(px)) <= (MESH_X_DIST) / 2)
return i; return px;
return -1; return -1;
} }
int8_t select_y_index(float y) { int8_t probe_index_y(float y) {
for (uint8_t i = MESH_NUM_Y_POINTS; i--;) for (int8_t py = MESH_NUM_Y_POINTS; py--;)
if (fabs(y - get_y(i)) <= (MESH_Y_DIST) / 2) if (fabs(y - get_probe_y(py)) <= (MESH_Y_DIST) / 2)
return i; return py;
return -1; return -1;
} }
@ -74,18 +86,18 @@
} }
float get_z(float x0, float y0) { float get_z(float x0, float y0) {
int8_t x_index = select_x_index(x0); int8_t cx = cel_index_x(x0),
int8_t y_index = select_y_index(y0); cy = cel_index_y(y0);
if (x_index < 0 || y_index < 0) return z_offset; if (cx < 0 || cy < 0) return z_offset;
float z1 = calc_z0(x0, float z1 = calc_z0(x0,
get_x(x_index), z_values[y_index][x_index], get_probe_x(cx), z_values[cy][cx],
get_x(x_index + 1), z_values[y_index][x_index + 1]); get_probe_x(cx + 1), z_values[cy][cx + 1]);
float z2 = calc_z0(x0, float z2 = calc_z0(x0,
get_x(x_index), z_values[y_index + 1][x_index], get_probe_x(cx), z_values[cy + 1][cx],
get_x(x_index + 1), z_values[y_index + 1][x_index + 1]); get_probe_x(cx + 1), z_values[cy + 1][cx + 1]);
float z0 = calc_z0(y0, float z0 = calc_z0(y0,
get_y(y_index), z1, get_probe_y(cy), z1,
get_y(y_index + 1), z2); get_probe_y(cy + 1), z2);
return z0 + z_offset; return z0 + z_offset;
} }
}; };

6
Marlin/ultralcd.cpp
View File

@ -1013,9 +1013,9 @@ void lcd_cooldown() {
lcd_goto_menu(_lcd_level_bed_moving); lcd_goto_menu(_lcd_level_bed_moving);
// _mbl_goto_xy runs the menu loop until the move is done // _mbl_goto_xy runs the menu loop until the move is done
int8_t ix, iy; int8_t px, py;
mbl.zigzag(_lcd_level_bed_position, ix, iy); mbl.zigzag(_lcd_level_bed_position, px, py);
_mbl_goto_xy(mbl.get_x(ix), mbl.get_y(iy)); _mbl_goto_xy(mbl.get_probe_x(px), mbl.get_probe_y(py));
// After the blocking function returns, change menus // After the blocking function returns, change menus
lcd_goto_menu(_lcd_level_bed_get_z); lcd_goto_menu(_lcd_level_bed_get_z);

Write Preview
Loading…
Cancel
Save