Merge pull request #673 from fsantini/ErikZalm
Improvements to the auto bed leveling feature
This commit is contained in:
commit
89a304fd98
@ -367,6 +367,15 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
|
||||
|
||||
#endif
|
||||
|
||||
// with accurate bed leveling, the bed is sampled in a ACCURATE_BED_LEVELING_POINTSxACCURATE_BED_LEVELING_POINTS grid and least squares solution is calculated
|
||||
// Note: this feature occupies 10'206 byte
|
||||
#define ACCURATE_BED_LEVELING
|
||||
|
||||
#ifdef ACCURATE_BED_LEVELING
|
||||
// I wouldn't see a reason to go above 3 (=9 probing points on the bed)
|
||||
#define ACCURATE_BED_LEVELING_POINTS 2
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
@ -31,6 +31,9 @@
|
||||
|
||||
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||
#include "vector_3.h"
|
||||
#ifdef ACCURATE_BED_LEVELING
|
||||
#include "qr_solve.h"
|
||||
#endif
|
||||
#endif // ENABLE_AUTO_BED_LEVELING
|
||||
|
||||
#include "ultralcd.h"
|
||||
@ -802,6 +805,31 @@ static void axis_is_at_home(int axis) {
|
||||
}
|
||||
|
||||
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||
#ifdef ACCURATE_BED_LEVELING
|
||||
static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
|
||||
{
|
||||
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
|
||||
planeNormal.debug("planeNormal");
|
||||
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
||||
//bedLevel.debug("bedLevel");
|
||||
|
||||
//plan_bed_level_matrix.debug("bed level before");
|
||||
//vector_3 uncorrected_position = plan_get_position_mm();
|
||||
//uncorrected_position.debug("position before");
|
||||
|
||||
vector_3 corrected_position = plan_get_position();
|
||||
// corrected_position.debug("position after");
|
||||
current_position[X_AXIS] = corrected_position.x;
|
||||
current_position[Y_AXIS] = corrected_position.y;
|
||||
current_position[Z_AXIS] = corrected_position.z;
|
||||
|
||||
// but the bed at 0 so we don't go below it.
|
||||
current_position[Z_AXIS] = -Z_PROBE_OFFSET_FROM_EXTRUDER;
|
||||
|
||||
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
||||
}
|
||||
|
||||
#else
|
||||
static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yFront, float z_at_xLeft_yBack) {
|
||||
plan_bed_level_matrix.set_to_identity();
|
||||
|
||||
@ -811,11 +839,11 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
|
||||
|
||||
vector_3 xPositive = (xRightyFront - xLeftyFront).get_normal();
|
||||
vector_3 yPositive = (xLeftyBack - xLeftyFront).get_normal();
|
||||
vector_3 planeNormal = vector_3::cross(yPositive, xPositive).get_normal();
|
||||
vector_3 planeNormal = vector_3::cross(xPositive, yPositive).get_normal();
|
||||
|
||||
//planeNormal.debug("planeNormal");
|
||||
//yPositive.debug("yPositive");
|
||||
matrix_3x3 bedLevel = matrix_3x3::create_look_at(planeNormal, yPositive);
|
||||
plan_bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
|
||||
//bedLevel.debug("bedLevel");
|
||||
|
||||
//plan_bed_level_matrix.debug("bed level before");
|
||||
@ -823,7 +851,6 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
|
||||
//uncorrected_position.debug("position before");
|
||||
|
||||
// and set our bed level equation to do the right thing
|
||||
plan_bed_level_matrix = matrix_3x3::create_inverse(bedLevel);
|
||||
//plan_bed_level_matrix.debug("bed level after");
|
||||
|
||||
vector_3 corrected_position = plan_get_position();
|
||||
@ -837,6 +864,7 @@ static void set_bed_level_equation(float z_at_xLeft_yFront, float z_at_xRight_yF
|
||||
|
||||
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
|
||||
}
|
||||
#endif // ACCURATE_BED_LEVELING
|
||||
|
||||
static void run_z_probe() {
|
||||
plan_bed_level_matrix.set_to_identity();
|
||||
@ -1325,6 +1353,98 @@ void process_commands()
|
||||
setup_for_endstop_move();
|
||||
|
||||
feedrate = homing_feedrate[Z_AXIS];
|
||||
#ifdef ACCURATE_BED_LEVELING
|
||||
|
||||
int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (ACCURATE_BED_LEVELING_POINTS-1);
|
||||
int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (ACCURATE_BED_LEVELING_POINTS-1);
|
||||
|
||||
|
||||
// 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
|
||||
|
||||
// "A" matrix of the linear system of equations
|
||||
double eqnAMatrix[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS*3];
|
||||
// "B" vector of Z points
|
||||
double eqnBVector[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS];
|
||||
|
||||
|
||||
int probePointCounter = 0;
|
||||
bool zig = true;
|
||||
|
||||
for (int yProbe=FRONT_PROBE_BED_POSITION; yProbe <= BACK_PROBE_BED_POSITION; yProbe += yGridSpacing)
|
||||
{
|
||||
int xProbe, xInc;
|
||||
if (zig)
|
||||
{
|
||||
xProbe = LEFT_PROBE_BED_POSITION;
|
||||
//xEnd = RIGHT_PROBE_BED_POSITION;
|
||||
xInc = xGridSpacing;
|
||||
zig = false;
|
||||
} else // zag
|
||||
{
|
||||
xProbe = RIGHT_PROBE_BED_POSITION;
|
||||
//xEnd = LEFT_PROBE_BED_POSITION;
|
||||
xInc = -xGridSpacing;
|
||||
zig = true;
|
||||
}
|
||||
|
||||
for (int xCount=0; xCount < ACCURATE_BED_LEVELING_POINTS; xCount++)
|
||||
{
|
||||
if (probePointCounter == 0)
|
||||
{
|
||||
// raise before probing
|
||||
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BEFORE_PROBING);
|
||||
} else
|
||||
{
|
||||
// raise extruder
|
||||
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
||||
}
|
||||
|
||||
|
||||
do_blocking_move_to(xProbe - X_PROBE_OFFSET_FROM_EXTRUDER, yProbe - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
|
||||
|
||||
engage_z_probe(); // Engage Z Servo endstop if available
|
||||
run_z_probe();
|
||||
eqnBVector[probePointCounter] = current_position[Z_AXIS];
|
||||
retract_z_probe();
|
||||
|
||||
SERIAL_PROTOCOLPGM("Bed x: ");
|
||||
SERIAL_PROTOCOL(xProbe);
|
||||
SERIAL_PROTOCOLPGM(" y: ");
|
||||
SERIAL_PROTOCOL(yProbe);
|
||||
SERIAL_PROTOCOLPGM(" z: ");
|
||||
SERIAL_PROTOCOL(current_position[Z_AXIS]);
|
||||
SERIAL_PROTOCOLPGM("\n");
|
||||
|
||||
eqnAMatrix[probePointCounter + 0*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = xProbe;
|
||||
eqnAMatrix[probePointCounter + 1*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = yProbe;
|
||||
eqnAMatrix[probePointCounter + 2*ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS] = 1;
|
||||
probePointCounter++;
|
||||
xProbe += xInc;
|
||||
}
|
||||
}
|
||||
clean_up_after_endstop_move();
|
||||
|
||||
// solve lsq problem
|
||||
double *plane_equation_coefficients = qr_solve(ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS, 3, eqnAMatrix, eqnBVector);
|
||||
|
||||
SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
|
||||
SERIAL_PROTOCOL(plane_equation_coefficients[0]);
|
||||
SERIAL_PROTOCOLPGM(" b: ");
|
||||
SERIAL_PROTOCOL(plane_equation_coefficients[1]);
|
||||
SERIAL_PROTOCOLPGM(" d: ");
|
||||
SERIAL_PROTOCOLLN(plane_equation_coefficients[2]);
|
||||
|
||||
|
||||
set_bed_level_equation_lsq(plane_equation_coefficients);
|
||||
|
||||
free(plane_equation_coefficients);
|
||||
|
||||
#else // ACCURATE_BED_LEVELING not defined
|
||||
|
||||
|
||||
// prob 1
|
||||
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], Z_RAISE_BEFORE_PROBING);
|
||||
@ -1382,6 +1502,8 @@ void process_commands()
|
||||
|
||||
set_bed_level_equation(z_at_xLeft_yFront, z_at_xRight_yFront, z_at_xLeft_yBack);
|
||||
|
||||
|
||||
#endif // ACCURATE_BED_LEVELING
|
||||
st_synchronize();
|
||||
|
||||
// The following code correct the Z height difference from z-probe position and hotend tip position.
|
||||
|
@ -942,7 +942,7 @@ vector_3 plan_get_position() {
|
||||
|
||||
//position.debug("in plan_get position");
|
||||
//plan_bed_level_matrix.debug("in plan_get bed_level");
|
||||
matrix_3x3 inverse = matrix_3x3::create_inverse(plan_bed_level_matrix);
|
||||
matrix_3x3 inverse = matrix_3x3::transpose(plan_bed_level_matrix);
|
||||
//inverse.debug("in plan_get inverse");
|
||||
position.apply_rotation(inverse);
|
||||
//position.debug("after rotation");
|
||||
|
1932
Marlin/qr_solve.cpp
Normal file
1932
Marlin/qr_solve.cpp
Normal file
File diff suppressed because it is too large
Load Diff
22
Marlin/qr_solve.h
Normal file
22
Marlin/qr_solve.h
Normal file
@ -0,0 +1,22 @@
|
||||
#include "Configuration.h"
|
||||
|
||||
#ifdef ACCURATE_BED_LEVELING
|
||||
|
||||
void daxpy ( int n, double da, double dx[], int incx, double dy[], int incy );
|
||||
double ddot ( int n, double dx[], int incx, double dy[], int incy );
|
||||
double dnrm2 ( int n, double x[], int incx );
|
||||
void dqrank ( double a[], int lda, int m, int n, double tol, int *kr,
|
||||
int jpvt[], double qraux[] );
|
||||
void dqrdc ( double a[], int lda, int n, int p, double qraux[], int jpvt[],
|
||||
double work[], int job );
|
||||
int dqrls ( double a[], int lda, int m, int n, double tol, int *kr, double b[],
|
||||
double x[], double rsd[], int jpvt[], double qraux[], int itask );
|
||||
void dqrlss ( double a[], int lda, int m, int n, int kr, double b[], double x[],
|
||||
double rsd[], int jpvt[], double qraux[] );
|
||||
int dqrsl ( double a[], int lda, int n, int k, double qraux[], double y[],
|
||||
double qy[], double qty[], double b[], double rsd[], double ab[], int job );
|
||||
void dscal ( int n, double sa, double x[], int incx );
|
||||
void dswap ( int n, double x[], int incx, double y[], int incy );
|
||||
double *qr_solve ( int m, int n, double a[], double b[] );
|
||||
|
||||
#endif
|
@ -127,57 +127,32 @@ void matrix_3x3::set_to_identity()
|
||||
matrix[6] = 0; matrix[7] = 0; matrix[8] = 1;
|
||||
}
|
||||
|
||||
matrix_3x3 matrix_3x3::create_look_at(vector_3 target, vector_3 up)
|
||||
matrix_3x3 matrix_3x3::create_look_at(vector_3 target)
|
||||
{
|
||||
// There are lots of examples of look at code on the internet that don't do all these noramize and also find the position
|
||||
// through several dot products. The problem with them is that they have a bit of error in that all the vectors arn't normal and need to be.
|
||||
vector_3 z_row = vector_3(-target.x, -target.y, -target.z).get_normal();
|
||||
vector_3 x_row = vector_3::cross(up, z_row).get_normal();
|
||||
vector_3 y_row = vector_3::cross(z_row, x_row).get_normal();
|
||||
vector_3 z_row = target.get_normal();
|
||||
vector_3 x_row = vector_3(1, 0, -target.x/target.z).get_normal();
|
||||
vector_3 y_row = vector_3(0, 1, -target.y/target.z).get_normal();
|
||||
|
||||
// x_row.debug("x_row");
|
||||
// y_row.debug("y_row");
|
||||
// z_row.debug("z_row");
|
||||
|
||||
matrix_3x3 rot = matrix_3x3::create_from_rows(vector_3(x_row.x, y_row.x, z_row.x),
|
||||
vector_3(x_row.y, y_row.y, z_row.y),
|
||||
vector_3(x_row.z, y_row.z, z_row.z));
|
||||
|
||||
// create the matrix already correctly transposed
|
||||
matrix_3x3 rot = matrix_3x3::create_from_rows(x_row, y_row, z_row);
|
||||
|
||||
// rot.debug("rot");
|
||||
return rot;
|
||||
}
|
||||
|
||||
matrix_3x3 matrix_3x3::create_inverse(matrix_3x3 original)
|
||||
|
||||
matrix_3x3 matrix_3x3::transpose(matrix_3x3 original)
|
||||
{
|
||||
//original.debug("original");
|
||||
float* A = original.matrix;
|
||||
float determinant =
|
||||
+ A[0 * 3 + 0] * (A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2])
|
||||
- A[0 * 3 + 1] * (A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0])
|
||||
+ A[0 * 3 + 2] * (A[1 * 3 + 0] * A[2 * 3 + 1] - A[1 * 3 + 1] * A[2 * 3 + 0]);
|
||||
matrix_3x3 inverse;
|
||||
inverse.matrix[0 * 3 + 0] = +(A[1 * 3 + 1] * A[2 * 3 + 2] - A[2 * 3 + 1] * A[1 * 3 + 2]) / determinant;
|
||||
inverse.matrix[0 * 3 + 1] = -(A[0 * 3 + 1] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 1]) / determinant;
|
||||
inverse.matrix[0 * 3 + 2] = +(A[0 * 3 + 1] * A[1 * 3 + 2] - A[0 * 3 + 2] * A[1 * 3 + 1]) / determinant;
|
||||
inverse.matrix[1 * 3 + 0] = -(A[1 * 3 + 0] * A[2 * 3 + 2] - A[1 * 3 + 2] * A[2 * 3 + 0]) / determinant;
|
||||
inverse.matrix[1 * 3 + 1] = +(A[0 * 3 + 0] * A[2 * 3 + 2] - A[0 * 3 + 2] * A[2 * 3 + 0]) / determinant;
|
||||
inverse.matrix[1 * 3 + 2] = -(A[0 * 3 + 0] * A[1 * 3 + 2] - A[1 * 3 + 0] * A[0 * 3 + 2]) / determinant;
|
||||
inverse.matrix[2 * 3 + 0] = +(A[1 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[1 * 3 + 1]) / determinant;
|
||||
inverse.matrix[2 * 3 + 1] = -(A[0 * 3 + 0] * A[2 * 3 + 1] - A[2 * 3 + 0] * A[0 * 3 + 1]) / determinant;
|
||||
inverse.matrix[2 * 3 + 2] = +(A[0 * 3 + 0] * A[1 * 3 + 1] - A[1 * 3 + 0] * A[0 * 3 + 1]) / determinant;
|
||||
|
||||
vector_3 row0 = vector_3(inverse.matrix[0 * 3 + 0], inverse.matrix[0 * 3 + 1], inverse.matrix[0 * 3 + 2]);
|
||||
vector_3 row1 = vector_3(inverse.matrix[1 * 3 + 0], inverse.matrix[1 * 3 + 1], inverse.matrix[1 * 3 + 2]);
|
||||
vector_3 row2 = vector_3(inverse.matrix[2 * 3 + 0], inverse.matrix[2 * 3 + 1], inverse.matrix[2 * 3 + 2]);
|
||||
|
||||
row0.normalize();
|
||||
row1.normalize();
|
||||
row2.normalize();
|
||||
|
||||
inverse = matrix_3x3::create_from_rows(row0, row1, row2);
|
||||
|
||||
//inverse.debug("inverse");
|
||||
return inverse;
|
||||
matrix_3x3 new_matrix;
|
||||
new_matrix.matrix[0] = original.matrix[0]; new_matrix.matrix[1] = original.matrix[3]; new_matrix.matrix[2] = original.matrix[6];
|
||||
new_matrix.matrix[3] = original.matrix[1]; new_matrix.matrix[4] = original.matrix[4]; new_matrix.matrix[5] = original.matrix[7];
|
||||
new_matrix.matrix[6] = original.matrix[2]; new_matrix.matrix[7] = original.matrix[5]; new_matrix.matrix[8] = original.matrix[8];
|
||||
return new_matrix;
|
||||
}
|
||||
|
||||
void matrix_3x3::debug(char* title)
|
||||
|
@ -47,8 +47,8 @@ struct matrix_3x3
|
||||
float matrix[9];
|
||||
|
||||
static matrix_3x3 create_from_rows(vector_3 row_0, vector_3 row_1, vector_3 row_2);
|
||||
static matrix_3x3 create_look_at(vector_3 target, vector_3 up);
|
||||
static matrix_3x3 create_inverse(matrix_3x3 original);
|
||||
static matrix_3x3 create_look_at(vector_3 target);
|
||||
static matrix_3x3 transpose(matrix_3x3 original);
|
||||
|
||||
void set_to_identity();
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user