MPCNC/Firmware
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fix up bed leveling code

Browse Source 
- Init `zprobe_zoffset`
- Remove `current_position[Z_AXIS] = zprobe_zoffset` lines from the
`set_bed_level_equation_*` functions
- Apply standards to `mesh_bed_leveling` files
- Document `MESH_BED_LEVELING`
This commit is contained in:
Scott Lahteine 2015-03-25 20:36:24 -07:00
parent c2ba5d0c09
commit 96b5da7198
4 changed files with 173 additions and 184 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Marlin/ConfigurationStore.cpp
View File

@ -11,7 +11,7 @@
* max_acceleration_units_per_sq_second (x4) * max_acceleration_units_per_sq_second (x4)
* acceleration * acceleration
* retract_acceleration * retract_acceleration
* travel_aceeleration * travel_acceleration
* minimumfeedrate * minimumfeedrate
* mintravelfeedrate * mintravelfeedrate
* minsegmenttime * minsegmenttime

271
Marlin/Marlin_main.cpp
View File

@ -255,7 +255,7 @@ float home_offset[3] = { 0, 0, 0 };
float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS }; float min_pos[3] = { X_MIN_POS, Y_MIN_POS, Z_MIN_POS };
float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS }; float max_pos[3] = { X_MAX_POS, Y_MAX_POS, Z_MAX_POS };
bool axis_known_position[3] = { false, false, false }; bool axis_known_position[3] = { false, false, false };
float zprobe_zoffset; float zprobe_zoffset = -Z_PROBE_OFFSET_FROM_EXTRUDER;
// Extruder offset // Extruder offset
#if EXTRUDERS > 1 #if EXTRUDERS > 1
@ -1092,9 +1092,6 @@ static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
current_position[Y_AXIS] = corrected_position.y; current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = corrected_position.z; current_position[Z_AXIS] = corrected_position.z;
// put the bed at 0 so we don't go below it.
current_position[Z_AXIS] = zprobe_zoffset; // in the lsq we reach here after raising the extruder due to the loop structure
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
} }
#endif #endif
@ -1121,9 +1118,6 @@ static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float
current_position[Y_AXIS] = corrected_position.y; current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = corrected_position.z; current_position[Z_AXIS] = corrected_position.z;
// put the bed at 0 so we don't go below it.
current_position[Z_AXIS] = zprobe_zoffset;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
} }
@ -2010,8 +2004,19 @@ inline void gcode_G28() {
endstops_hit_on_purpose(); endstops_hit_on_purpose();
} }
#if defined(MESH_BED_LEVELING) #ifdef MESH_BED_LEVELING
/**
* G29: Mesh-based Z-Probe, probes a grid and produces a
* mesh to compensate for variable bed height
*
* Parameters With MESH_BED_LEVELING:
*
* S0 Produce a mesh report
* S1 Start probing mesh points
* S2 Probe the next mesh point
*
*/
inline void gcode_G29() { inline void gcode_G29() {
static int probe_point = -1; static int probe_point = -1;
int state = 0; int state = 0;
@ -2053,7 +2058,7 @@ inline void gcode_G28() {
} else if (state == 2) { // Goto next point } else if (state == 2) { // Goto next point
if (probe_point < 0) { if (probe_point < 0) {
SERIAL_PROTOCOLPGM("Mesh probing not started.\n"); SERIAL_PROTOCOLPGM("Start mesh probing with \"G29 S1\" first.\n");
return; return;
} }
int ix, iy; int ix, iy;
@ -2063,16 +2068,14 @@ inline void gcode_G28() {
} else { } else {
ix = (probe_point-1) % MESH_NUM_X_POINTS; ix = (probe_point-1) % MESH_NUM_X_POINTS;
iy = (probe_point-1) / MESH_NUM_X_POINTS; iy = (probe_point-1) / MESH_NUM_X_POINTS;
if (iy&1) { // Zig zag if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
ix = (MESH_NUM_X_POINTS - 1) - ix;
}
mbl.set_z(ix, iy, current_position[Z_AXIS]); mbl.set_z(ix, iy, current_position[Z_AXIS]);
current_position[Z_AXIS] = MESH_HOME_SEARCH_Z; current_position[Z_AXIS] = MESH_HOME_SEARCH_Z;
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder); plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
st_synchronize(); st_synchronize();
} }
if (probe_point == MESH_NUM_X_POINTS*MESH_NUM_Y_POINTS) { if (probe_point == MESH_NUM_X_POINTS * MESH_NUM_Y_POINTS) {
SERIAL_PROTOCOLPGM("Mesh done.\n"); SERIAL_PROTOCOLPGM("Mesh probing done.\n");
probe_point = -1; probe_point = -1;
mbl.active = 1; mbl.active = 1;
enquecommands_P(PSTR("G28")); enquecommands_P(PSTR("G28"));
@ -2080,9 +2083,7 @@ inline void gcode_G28() {
} }
ix = probe_point % MESH_NUM_X_POINTS; ix = probe_point % MESH_NUM_X_POINTS;
iy = probe_point / MESH_NUM_X_POINTS; iy = probe_point / MESH_NUM_X_POINTS;
if (iy&1) { // Zig zag if (iy & 1) ix = (MESH_NUM_X_POINTS - 1) - ix; // zig-zag
ix = (MESH_NUM_X_POINTS - 1) - ix;
}
current_position[X_AXIS] = mbl.get_x(ix); current_position[X_AXIS] = mbl.get_x(ix);
current_position[Y_AXIS] = mbl.get_y(iy); current_position[Y_AXIS] = mbl.get_y(iy);
plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder); plan_buffer_line(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS], homing_feedrate[X_AXIS]/60, active_extruder);
@ -2091,9 +2092,7 @@ inline void gcode_G28() {
} }
} }
#endif #elif defined(ENABLE_AUTO_BED_LEVELING)
#ifdef ENABLE_AUTO_BED_LEVELING
/** /**
* G29: Detailed Z-Probe, probes the bed at 3 or more points. * G29: Detailed Z-Probe, probes the bed at 3 or more points.
@ -2154,9 +2153,9 @@ inline void gcode_G28() {
#ifdef AUTO_BED_LEVELING_GRID #ifdef AUTO_BED_LEVELING_GRID
#ifndef DELTA #ifndef DELTA
bool do_topography_map = verbose_level > 2 || code_seen('T') || code_seen('t'); bool do_topography_map = verbose_level > 2 || code_seen('T') || code_seen('t');
#endif #endif
if (verbose_level > 0) if (verbose_level > 0)
SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling\n"); SERIAL_PROTOCOLPGM("G29 Auto Bed Leveling\n");
@ -2210,7 +2209,7 @@ inline void gcode_G28() {
#ifdef Z_PROBE_SLED #ifdef Z_PROBE_SLED
dock_sled(false); // engage (un-dock) the probe dock_sled(false); // engage (un-dock) the probe
#elif defined(Z_PROBE_ALLEN_KEY) #elif defined(Z_PROBE_ALLEN_KEY) //|| defined(SERVO_LEVELING)
engage_z_probe(); engage_z_probe();
#endif #endif
@ -2218,19 +2217,18 @@ inline void gcode_G28() {
#ifdef DELTA #ifdef DELTA
reset_bed_level(); reset_bed_level();
#else #else //!DELTA
// make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly //vector_3 corrected_position = plan_get_position_mm();
//vector_3 corrected_position = plan_get_position_mm(); //corrected_position.debug("position before G29");
//corrected_position.debug("position before G29"); plan_bed_level_matrix.set_to_identity();
plan_bed_level_matrix.set_to_identity(); vector_3 uncorrected_position = plan_get_position();
vector_3 uncorrected_position = plan_get_position(); //uncorrected_position.debug("position during G29");
//uncorrected_position.debug("position during G29"); current_position[X_AXIS] = uncorrected_position.x;
current_position[X_AXIS] = uncorrected_position.x; current_position[Y_AXIS] = uncorrected_position.y;
current_position[Y_AXIS] = uncorrected_position.y; current_position[Z_AXIS] = uncorrected_position.z;
current_position[Z_AXIS] = uncorrected_position.z; plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); #endif //!DELTA
#endif
setup_for_endstop_move(); setup_for_endstop_move();
@ -2242,26 +2240,24 @@ inline void gcode_G28() {
const int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points-1); const int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points-1);
const int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points-1); const int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points-1);
#ifndef DELTA #ifdef DELTA
// solve the plane equation ax + by + d = z delta_grid_spacing[0] = xGridSpacing;
// A is the matrix with rows [x y 1] for all the probed points delta_grid_spacing[1] = yGridSpacing;
// B is the vector of the Z positions float z_offset = Z_PROBE_OFFSET_FROM_EXTRUDER;
// the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0 if (code_seen(axis_codes[Z_AXIS])) z_offset += code_value();
// so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z #else // !DELTA
// solve the plane equation ax + by + d = z
// A is the matrix with rows [x y 1] for all the probed points
// B is the vector of the Z positions
// the normal vector to the plane is formed by the coefficients of the plane equation in the standard form, which is Vx*x+Vy*y+Vz*z+d = 0
// so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points; int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points;
double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
eqnBVector[abl2], // "B" vector of Z points eqnBVector[abl2], // "B" vector of Z points
mean = 0.0; mean = 0.0;
#endif // !DELTA
#else
delta_grid_spacing[0] = xGridSpacing;
delta_grid_spacing[1] = yGridSpacing;
float z_offset = Z_PROBE_OFFSET_FROM_EXTRUDER;
if (code_seen(axis_codes[Z_AXIS])) z_offset += code_value();
#endif
int probePointCounter = 0; int probePointCounter = 0;
bool zig = true; bool zig = true;
@ -2294,12 +2290,12 @@ inline void gcode_G28() {
float measured_z, float measured_z,
z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS; z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
#ifdef DELTA #ifdef DELTA
// Avoid probing the corners (outside the round or hexagon print surface) on a delta printer. // Avoid probing the corners (outside the round or hexagon print surface) on a delta printer.
float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe); float distance_from_center = sqrt(xProbe*xProbe + yProbe*yProbe);
if (distance_from_center > DELTA_PROBABLE_RADIUS) if (distance_from_center > DELTA_PROBABLE_RADIUS)
continue; continue;
#endif //DELTA #endif //DELTA
// Enhanced G29 - Do not retract servo between probes // Enhanced G29 - Do not retract servo between probes
ProbeAction act; ProbeAction act;
@ -2316,16 +2312,16 @@ inline void gcode_G28() {
measured_z = probe_pt(xProbe, yProbe, z_before, act, verbose_level); measured_z = probe_pt(xProbe, yProbe, z_before, act, verbose_level);
#ifndef DELTA #ifndef DELTA
mean += measured_z; mean += measured_z;
eqnBVector[probePointCounter] = measured_z; eqnBVector[probePointCounter] = measured_z;
eqnAMatrix[probePointCounter + 0 * abl2] = xProbe; eqnAMatrix[probePointCounter + 0 * abl2] = xProbe;
eqnAMatrix[probePointCounter + 1 * abl2] = yProbe; eqnAMatrix[probePointCounter + 1 * abl2] = yProbe;
eqnAMatrix[probePointCounter + 2 * abl2] = 1; eqnAMatrix[probePointCounter + 2 * abl2] = 1;
#else #else
bed_level[xCount][yCount] = measured_z + z_offset; bed_level[xCount][yCount] = measured_z + z_offset;
#endif #endif
probePointCounter++; probePointCounter++;
} //xProbe } //xProbe
@ -2333,60 +2329,61 @@ inline void gcode_G28() {
clean_up_after_endstop_move(); clean_up_after_endstop_move();
#ifndef DELTA #ifdef DELTA
// solve lsq problem extrapolate_unprobed_bed_level();
double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector); print_bed_level();
#else // !DELTA
// solve lsq problem
double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
mean /= abl2; mean /= abl2;
if (verbose_level) { if (verbose_level) {
SERIAL_PROTOCOLPGM("Eqn coefficients: a: "); SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8); SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
SERIAL_PROTOCOLPGM(" b: "); SERIAL_PROTOCOLPGM(" b: ");
SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8); SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8);
SERIAL_PROTOCOLPGM(" d: "); SERIAL_PROTOCOLPGM(" d: ");
SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8); SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8);
SERIAL_EOL;
if (verbose_level > 2) {
SERIAL_PROTOCOLPGM("Mean of sampled points: ");
SERIAL_PROTOCOL_F(mean, 8);
SERIAL_EOL; SERIAL_EOL;
if (verbose_level > 2) {
SERIAL_PROTOCOLPGM("Mean of sampled points: ");
SERIAL_PROTOCOL_F(mean, 8);
SERIAL_EOL;
}
} }
}
// Show the Topography map if enabled // Show the Topography map if enabled
if (do_topography_map) { if (do_topography_map) {
SERIAL_PROTOCOLPGM(" \nBed Height Topography: \n"); SERIAL_PROTOCOLPGM(" \nBed Height Topography: \n");
SERIAL_PROTOCOLPGM("+-----------+\n"); SERIAL_PROTOCOLPGM("+-----------+\n");
SERIAL_PROTOCOLPGM("|...Back....|\n"); SERIAL_PROTOCOLPGM("|...Back....|\n");
SERIAL_PROTOCOLPGM("|Left..Right|\n"); SERIAL_PROTOCOLPGM("|Left..Right|\n");
SERIAL_PROTOCOLPGM("|...Front...|\n"); SERIAL_PROTOCOLPGM("|...Front...|\n");
SERIAL_PROTOCOLPGM("+-----------+\n"); SERIAL_PROTOCOLPGM("+-----------+\n");
for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) { for (int yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--) {
for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) { for (int xx = 0; xx < auto_bed_leveling_grid_points; xx++) {
int ind = yy * auto_bed_leveling_grid_points + xx; int ind = yy * auto_bed_leveling_grid_points + xx;
float diff = eqnBVector[ind] - mean; float diff = eqnBVector[ind] - mean;
if (diff >= 0.0) if (diff >= 0.0)
SERIAL_PROTOCOLPGM(" +"); // Include + for column alignment SERIAL_PROTOCOLPGM(" +"); // Include + for column alignment
else else
SERIAL_PROTOCOLPGM(" "); SERIAL_PROTOCOLPGM(" ");
SERIAL_PROTOCOL_F(diff, 5); SERIAL_PROTOCOL_F(diff, 5);
} // xx } // xx
SERIAL_EOL;
} // yy
SERIAL_EOL; SERIAL_EOL;
} // yy
SERIAL_EOL;
} //do_topography_map } //do_topography_map
set_bed_level_equation_lsq(plane_equation_coefficients); set_bed_level_equation_lsq(plane_equation_coefficients);
free(plane_equation_coefficients); free(plane_equation_coefficients);
#else
extrapolate_unprobed_bed_level(); #endif // !DELTA
print_bed_level();
#endif
#else // !AUTO_BED_LEVELING_GRID #else // !AUTO_BED_LEVELING_GRID
@ -2409,33 +2406,33 @@ inline void gcode_G28() {
#endif // !AUTO_BED_LEVELING_GRID #endif // !AUTO_BED_LEVELING_GRID
#ifndef DELTA #ifndef DELTA
if (verbose_level > 0) if (verbose_level > 0)
plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:"); plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
// Correct the Z height difference from z-probe position and hotend tip position. // Correct the Z height difference from z-probe position and hotend tip position.
// The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend. // The Z height on homing is measured by Z-Probe, but the probe is quite far from the hotend.
// When the bed is uneven, this height must be corrected. // When the bed is uneven, this height must be corrected.
real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane) real_z = float(st_get_position(Z_AXIS)) / axis_steps_per_unit[Z_AXIS]; //get the real Z (since the auto bed leveling is already correcting the plane)
x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER; x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER; y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
z_tmp = current_position[Z_AXIS]; z_tmp = current_position[Z_AXIS];
apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset apply_rotation_xyz(plan_bed_level_matrix, x_tmp, y_tmp, z_tmp); //Apply the correction sending the probe offset
current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner. current_position[Z_AXIS] = z_tmp - real_z + current_position[Z_AXIS]; //The difference is added to current position and sent to planner.
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]); plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
#endif #endif
#ifdef Z_PROBE_SLED #ifdef Z_PROBE_SLED
dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel
#elif defined(Z_PROBE_ALLEN_KEY) #elif defined(Z_PROBE_ALLEN_KEY) //|| defined(SERVO_LEVELING)
retract_z_probe(); retract_z_probe();
#endif #endif
#ifdef Z_PROBE_END_SCRIPT #ifdef Z_PROBE_END_SCRIPT
enquecommands_P(PSTR(Z_PROBE_END_SCRIPT)); enquecommands_P(PSTR(Z_PROBE_END_SCRIPT));
st_synchronize(); st_synchronize();
#endif #endif
} }
#ifndef Z_PROBE_SLED #ifndef Z_PROBE_SLED

22
Marlin/mesh_bed_leveling.cpp
View File

@ -1,20 +1,16 @@
#include "mesh_bed_leveling.h" #include "mesh_bed_leveling.h"
#if defined(MESH_BED_LEVELING) #ifdef MESH_BED_LEVELING
mesh_bed_leveling mbl; mesh_bed_leveling mbl;
mesh_bed_leveling::mesh_bed_leveling() { mesh_bed_leveling::mesh_bed_leveling() { reset(); }
reset();
} void mesh_bed_leveling::reset() {
void mesh_bed_leveling::reset() {
for (int y=0; y<MESH_NUM_Y_POINTS; y++) {
for (int x=0; x<MESH_NUM_X_POINTS; x++) {
z_values[y][x] = 0;
}
}
active = 0; active = 0;
} for (int y = 0; y < MESH_NUM_Y_POINTS; y++)
for (int x = 0; x < MESH_NUM_X_POINTS; x++)
z_values[y][x] = 0;
}
#endif // MESH_BED_LEVELING #endif // MESH_BED_LEVELING

62
Marlin/mesh_bed_leveling.h
View File

@ -2,11 +2,11 @@
#if defined(MESH_BED_LEVELING) #if defined(MESH_BED_LEVELING)
#define MESH_X_DIST ((MESH_MAX_X - MESH_MIN_X)/(MESH_NUM_X_POINTS - 1)) #define MESH_X_DIST ((MESH_MAX_X - MESH_MIN_X)/(MESH_NUM_X_POINTS - 1))
#define MESH_Y_DIST ((MESH_MAX_Y - MESH_MIN_Y)/(MESH_NUM_Y_POINTS - 1)) #define MESH_Y_DIST ((MESH_MAX_Y - MESH_MIN_Y)/(MESH_NUM_Y_POINTS - 1))
class mesh_bed_leveling { class mesh_bed_leveling {
public: public:
uint8_t active; uint8_t active;
float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS]; float z_values[MESH_NUM_Y_POINTS][MESH_NUM_X_POINTS];
@ -14,48 +14,44 @@ public:
void reset(); void reset();
float get_x(int i) { return MESH_MIN_X + MESH_X_DIST*i; } float get_x(int i) { return MESH_MIN_X + MESH_X_DIST * i; }
float get_y(int i) { return MESH_MIN_Y + MESH_Y_DIST*i; } float get_y(int i) { return MESH_MIN_Y + MESH_Y_DIST * i; }
void set_z(int ix, int iy, float z) { z_values[iy][ix] = z; } void set_z(int ix, int iy, float z) { z_values[iy][ix] = z; }
int select_x_index(float x) { int select_x_index(float x) {
int i = 1; int i = 1;
while (x > get_x(i) && i < MESH_NUM_X_POINTS-1) { while (x > get_x(i) && i < MESH_NUM_X_POINTS-1) i++;
i++; return i - 1;
}
return i-1;
} }
int select_y_index(float y) { int select_y_index(float y) {
int i = 1; int i = 1;
while (y > get_y(i) && i < MESH_NUM_Y_POINTS-1) { while (y > get_y(i) && i < MESH_NUM_Y_POINTS - 1) i++;
i++; return i - 1;
}
return i-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) {
float delta_z = (z2 - z1)/(a2 - a1); float delta_z = (z2 - z1)/(a2 - a1);
float delta_a = a0 - a1; float delta_a = a0 - a1;
return z1 + delta_a * delta_z; return z1 + delta_a * delta_z;
} }
float get_z(float x0, float y0) { float get_z(float x0, float y0) {
int x_index = select_x_index(x0); int x_index = select_x_index(x0);
int y_index = select_y_index(y0); int y_index = select_y_index(y0);
float z1 = calc_z0(x0, float z1 = calc_z0(x0,
get_x(x_index), z_values[y_index][x_index], get_x(x_index), z_values[y_index][x_index],
get_x(x_index+1), z_values[y_index][x_index+1]); get_x(x_index+1), z_values[y_index][x_index+1]);
float z2 = calc_z0(x0, float z2 = calc_z0(x0,
get_x(x_index), z_values[y_index+1][x_index], 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_x(x_index+1), z_values[y_index+1][x_index+1]);
float z0 = calc_z0(y0, float z0 = calc_z0(y0,
get_y(y_index), z1, get_y(y_index), z1,
get_y(y_index+1), z2); get_y(y_index+1), z2);
return z0; return z0;
} }
}; };
extern mesh_bed_leveling mbl; extern mesh_bed_leveling mbl;
#endif // MESH_BED_LEVELING #endif // MESH_BED_LEVELING