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

Fix up bed leveling code 

- 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`
pull/1/head
Scott Lahteine 10 years ago
parent
c2ba5d0c09
commit
96b5da7198
4 changed files with 181 additions and 192 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. 2
      Marlin/ConfigurationStore.cpp
  2. 287
      Marlin/Marlin_main.cpp
  3. 22
      Marlin/mesh_bed_leveling.cpp
  4. 62
      Marlin/mesh_bed_leveling.h

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

287
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
int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points; // A is the matrix with rows [x y 1] for all the probed points
// B is the vector of the Z positions
double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations // 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
eqnBVector[abl2], // "B" vector of Z points // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
mean = 0.0;
int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points;
#else
delta_grid_spacing[0] = xGridSpacing; double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
delta_grid_spacing[1] = yGridSpacing; eqnBVector[abl2], // "B" vector of Z points
mean = 0.0;
float z_offset = Z_PROBE_OFFSET_FROM_EXTRUDER; #endif // !DELTA
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
mean /= abl2; // solve lsq problem
double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
if (verbose_level) {
SERIAL_PROTOCOLPGM("Eqn coefficients: a: "); mean /= abl2;
SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
SERIAL_PROTOCOLPGM(" b: "); if (verbose_level) {
SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8); SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
SERIAL_PROTOCOLPGM(" d: "); SERIAL_PROTOCOL_F(plane_equation_coefficients[0], 8);
SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8); SERIAL_PROTOCOLPGM(" b: ");
SERIAL_EOL; SERIAL_PROTOCOL_F(plane_equation_coefficients[1], 8);
if (verbose_level > 2) { SERIAL_PROTOCOLPGM(" d: ");
SERIAL_PROTOCOLPGM("Mean of sampled points: "); SERIAL_PROTOCOL_F(plane_equation_coefficients[2], 8);
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

Write Preview
Loading…
Cancel
Save