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

Merge pull request #3812 from thinkyhead/rc_mbl_index_finders 

Two index finding functions for MBL
pull/1/head
Scott Lahteine 9 years ago
parent
89e630b2fb 827738a790
commit
30f6b84561
5 changed files with 91 additions and 89 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. 94
      Marlin/Marlin_main.cpp
  2. 12
      Marlin/configuration_store.cpp
  3. 2
      Marlin/language.h
  4. 66
      Marlin/mesh_bed_leveling.h
  5. 6
      Marlin/ultralcd.cpp

94
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:
@ -5904,39 +5904,35 @@ inline void gcode_M410() { stepper.quick_stop(); }
* Use either 'M421 X<mm> Y<mm> Z<mm>' or 'M421 I<xindex> J<yindex> Z<mm>' * Use either 'M421 X<mm> Y<mm> Z<mm>' or 'M421 I<xindex> J<yindex> Z<mm>'
*/ */
inline void gcode_M421() { inline void gcode_M421() {
float x = 0, y = 0, z = 0; int8_t px, py;
int8_t i = 0, j = 0; float z = 0;
bool err = false, hasX, hasY, hasZ, hasI, hasJ; bool hasX, hasY, hasZ, hasI, hasJ;
if ((hasX = code_seen('X'))) x = code_value(); if ((hasX = code_seen('X'))) px = mbl.probe_index_x(code_value());
if ((hasY = code_seen('Y'))) y = code_value(); if ((hasY = code_seen('Y'))) py = mbl.probe_index_y(code_value());
if ((hasI = code_seen('I'))) i = code_value(); if ((hasI = code_seen('I'))) px = code_value();
if ((hasJ = code_seen('J'))) j = code_value(); if ((hasJ = code_seen('J'))) py = code_value();
if ((hasZ = code_seen('Z'))) z = code_value(); if ((hasZ = code_seen('Z'))) z = code_value();
if (hasX && hasY && hasZ) { if (hasX && hasY && hasZ) {
int8_t ix = mbl.select_x_index(x), if (px >= 0 && py >= 0)
iy = mbl.select_y_index(y); mbl.set_z(px, py, z);
if (ix >= 0 && iy >= 0)
mbl.set_z(ix, iy, 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_PARAMETERS);
} }
} }
@ -7303,52 +7299,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

12
Marlin/configuration_store.cpp
View File

@ -62,7 +62,7 @@
* 201 z_offset (float) (added in V23) * 201 z_offset (float) (added in V23)
* 205 mesh_num_x (uint8 as set in firmware) * 205 mesh_num_x (uint8 as set in firmware)
* 206 mesh_num_y (uint8 as set in firmware) * 206 mesh_num_y (uint8 as set in firmware)
* 207 M421 XYZ z_values[][] (float x9, by default) * 207 G29 S3 XYZ z_values[][] (float x9, by default)
* *
* AUTO BED LEVELING * AUTO BED LEVELING
* 243 M851 zprobe_zoffset (float) * 243 M851 zprobe_zoffset (float)
@ -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 = 1; py <= MESH_NUM_Y_POINTS; py++) {
for (uint8_t x = 0; x < MESH_NUM_X_POINTS; x++) { for (uint8_t px = 1; px <= MESH_NUM_X_POINTS; px++) {
CONFIG_ECHO_START; CONFIG_ECHO_START;
SERIAL_ECHOPAIR(" M421 X", mbl.get_x(x)); SERIAL_ECHOPAIR(" G29 S3 X", px);
SERIAL_ECHOPAIR(" Y", mbl.get_y(y)); SERIAL_ECHOPAIR(" Y", py);
SERIAL_ECHOPGM(" Z"); SERIAL_ECHOPGM(" Z");
SERIAL_PROTOCOL_F(mbl.z_values[y][x], 5); SERIAL_PROTOCOL_F(mbl.z_values[py-1][px-1], 5);
SERIAL_EOL; SERIAL_EOL;
} }
} }

2
Marlin/language.h
View File

@ -147,7 +147,7 @@
#define MSG_Z2_MAX "z2_max: " #define MSG_Z2_MAX "z2_max: "
#define MSG_Z_PROBE "z_probe: " #define MSG_Z_PROBE "z_probe: "
#define MSG_ERR_MATERIAL_INDEX "M145 S<index> out of range (0-1)" #define MSG_ERR_MATERIAL_INDEX "M145 S<index> out of range (0-1)"
#define MSG_ERR_M421_REQUIRES_XYZ "M421 requires XYZ or IJZ parameters" #define MSG_ERR_M421_PARAMETERS "M421 requires XYZ or IJZ parameters"
#define MSG_ERR_MESH_XY "Mesh XY or IJ cannot be resolved" #define MSG_ERR_MESH_XY "Mesh XY or IJ cannot be resolved"
#define MSG_ERR_M428_TOO_FAR "Too far from reference point" #define MSG_ERR_M428_TOO_FAR "Too far from reference point"
#define MSG_ERR_M303_DISABLED "PIDTEMP disabled" #define MSG_ERR_M303_DISABLED "PIDTEMP disabled"

66
Marlin/mesh_bed_leveling.h
View File

@ -37,34 +37,40 @@
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(const int8_t px, const int8_t py, const 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 = int(x - (MESH_MIN_X)) / (MESH_X_DIST);
return constrain(cx, 0, (MESH_NUM_X_POINTS) - 2);
}
int8_t cel_index_y(float y) {
int8_t cy = int(y - (MESH_MIN_Y)) / (MESH_Y_DIST);
return constrain(cy, 0, (MESH_NUM_Y_POINTS) - 2);
} }
int8_t select_x_index(float x) { int8_t probe_index_x(float x) {
for (uint8_t i = MESH_NUM_X_POINTS; i--;) int8_t px = int(x - (MESH_MIN_X) + (MESH_X_DIST) / 2) / (MESH_X_DIST);
if (fabs(x - get_x(i)) <= (MESH_X_DIST) / 2) return (px >= 0 && px < (MESH_NUM_X_POINTS)) ? px : -1;
return i;
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--;) int8_t py = int(y - (MESH_MIN_Y) + (MESH_Y_DIST) / 2) / (MESH_Y_DIST);
if (fabs(y - get_y(i)) <= (MESH_Y_DIST) / 2) return (py >= 0 && py < (MESH_NUM_Y_POINTS)) ? py : -1;
return i;
return -1;
} }
float calc_z0(float a0, float a1, float z1, float a2, float z2) { float calc_z0(float a0, float a1, float z1, float a2, float z2) {
@ -74,18 +80,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