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
Split 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;
}
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:
@ -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>'
*/
inline void gcode_M421() {
float x = 0, y = 0, z = 0;
int8_t i = 0, j = 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();
int8_t px, py;
float z = 0;
bool hasX, hasY, hasZ, hasI, hasJ;
if ((hasX = code_seen('X'))) px = mbl.probe_index_x(code_value());
if ((hasY = code_seen('Y'))) py = mbl.probe_index_y(code_value());
if ((hasI = code_seen('I'))) px = code_value();
if ((hasJ = code_seen('J'))) py = code_value();
if ((hasZ = code_seen('Z'))) z = code_value();
if (hasX && hasY && hasZ) {
int8_t ix = mbl.select_x_index(x),
iy = mbl.select_y_index(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);
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();
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

12
Marlin/configuration_store.cpp
View File

@ -62,7 +62,7 @@
* 201 z_offset (float) (added in V23)
* 205 mesh_num_x (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
* 243 M851 zprobe_zoffset (float)
@ -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 = 1; py <= MESH_NUM_Y_POINTS; py++) {
for (uint8_t px = 1; 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(" G29 S3 X", px);
SERIAL_ECHOPAIR(" Y", py);
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;
}
}

2
Marlin/language.h
View File

@ -147,7 +147,7 @@
#define MSG_Z2_MAX "z2_max: "
#define MSG_Z_PROBE "z_probe: "
#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_M428_TOO_FAR "Too far from reference point"
#define MSG_ERR_M303_DISABLED "PIDTEMP disabled"

66
Marlin/mesh_bed_leveling.h
View File

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

6
Marlin/ultralcd.cpp
View File

@ -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);

Write Preview
Loading…
Cancel
Save