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`
This commit is contained in:
parent
c2ba5d0c09
commit
96b5da7198
@ -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
|
||||||
|
@ -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.
|
||||||
@ -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,9 +2217,8 @@ 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();
|
||||||
@ -2230,7 +2228,7 @@ inline void gcode_G28() {
|
|||||||
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
|
#endif //!DELTA
|
||||||
|
|
||||||
setup_for_endstop_move();
|
setup_for_endstop_move();
|
||||||
|
|
||||||
@ -2242,7 +2240,12 @@ 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
|
||||||
|
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();
|
||||||
|
#else // !DELTA
|
||||||
// solve the plane equation ax + by + d = z
|
// solve the plane equation ax + by + d = z
|
||||||
// A is the matrix with rows [x y 1] for all the probed points
|
// A is the matrix with rows [x y 1] for all the probed points
|
||||||
// B is the vector of the Z positions
|
// B is the vector of the Z positions
|
||||||
@ -2254,14 +2257,7 @@ inline void gcode_G28() {
|
|||||||
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;
|
||||||
@ -2333,7 +2329,10 @@ inline void gcode_G28() {
|
|||||||
|
|
||||||
clean_up_after_endstop_move();
|
clean_up_after_endstop_move();
|
||||||
|
|
||||||
#ifndef DELTA
|
#ifdef DELTA
|
||||||
|
extrapolate_unprobed_bed_level();
|
||||||
|
print_bed_level();
|
||||||
|
#else // !DELTA
|
||||||
// solve lsq problem
|
// solve lsq problem
|
||||||
double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
|
double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
|
||||||
|
|
||||||
@ -2383,10 +2382,8 @@ inline void gcode_G28() {
|
|||||||
|
|
||||||
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
|
||||||
|
|
||||||
@ -2428,7 +2425,7 @@ inline void gcode_G28() {
|
|||||||
|
|
||||||
#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
|
||||||
|
|
||||||
|
@ -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++) {
|
active = 0;
|
||||||
for (int x=0; x<MESH_NUM_X_POINTS; x++) {
|
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;
|
z_values[y][x] = 0;
|
||||||
}
|
}
|
||||||
}
|
|
||||||
active = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
#endif // MESH_BED_LEVELING
|
#endif // MESH_BED_LEVELING
|
||||||
|
@ -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,24 +14,20 @@ 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) {
|
||||||
@ -54,8 +50,8 @@ public:
|
|||||||
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
|
||||||
|
Loading…
Reference in New Issue
Block a user