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

Simplify 3-point probing using new code only

Browse Source
This commit is contained in:
Gabe Rosenhouse 2014-02-19 21:12:39 -08:00
parent 3b718b816c
commit 174b8d99d5
4 changed files with 57 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

83
Marlin/Configuration.h
View File

@ -335,50 +335,49 @@ const bool Z_MAX_ENDSTOP_INVERTING = true; // set to true to invert the logic of
#ifdef ENABLE_AUTO_BED_LEVELING
// There are 3 different ways to pick the X and Y locations to probe:
// 1. Basic 3-point probe at left-back, left-front, and right-front corners of a rectangle
// 2. Probe all points of a 2D lattice, defined by a rectangle and ACCURATE_BED_LEVELING_POINTS
// 3. 3-point probe at 3 arbitrary points that don't form a line.
// There are 2 different ways to pick the X and Y locations to probe:
// To enable mode 1:
// - #define ENABLE_AUTO_BED_LEVELING
// - Set the XXXX_PROBE_BED_POSITION values below
// - Don't define AUTO_BED_LEVELING_ANY_POINTS or ACCURATE_BED_LEVELING
// - "grid" mode
// Probe every point in a rectangular grid
// You must specify the rectangle, and the density of sample points
// This mode is preferred because there are more measurements.
// It used to be called ACCURATE_BED_LEVELING but "grid" is more descriptive
// To enable mode 2:
// - #define ENABLE_AUTO_BED_LEVELING
// - Set the XXXX_PROBE_BED_POSITION values below
// - #define ACCURATE_BED_LEVELING
// - Set the ACCURATE_BED_LEVELING_POINTS to your desired density
// - "3-point" mode
// Probe 3 arbitrary points on the bed (that aren't colinear)
// You must specify the X & Y coordinates of all 3 points
// To enable mode 3:
// - #define ENABLE_AUTO_BED_LEVELING
// - #define AUTO_BED_LEVELING_ANY_POINTS
// - Set the ABL_PROBE_PT_XXXX values below
// - Comment out (undefine) ACCURATE_BED_LEVELING since that is incompatible
#define AUTO_BED_LEVELING_GRID
// with AUTO_BED_LEVELING_GRID, the bed is sampled in a
// AUTO_BED_LEVELING_GRID_POINTSxAUTO_BED_LEVELING_GRID_POINTS grid
// and least squares solution is calculated
// Note: this feature occupies 10'206 byte
#ifdef AUTO_BED_LEVELING_GRID
// Mode 3: Enable auto bed leveling at any 3 points that aren't colinear
// #define AUTO_BED_LEVELING_ANY_POINTS
#ifdef AUTO_BED_LEVELING_ANY_POINTS
#define ABL_PROBE_PT_1_X 15
#define ABL_PROBE_PT_1_Y 15
#define ABL_PROBE_PT_2_X 25
#define ABL_PROBE_PT_2_Y 75
#define ABL_PROBE_PT_3_X 125
#define ABL_PROBE_PT_3_Y 25
#else // not AUTO_BED_LEVELING_ANY_POINTS
// Modes 1 & 2:
// For mode 1, probing happens at left-back, left-front, and right-front corners
// For mode 2, probing happens at lattice points within this rectangle (see ACCURATE_BED_LEVELING_POINTS)
// set the rectangle in which to probe
#define LEFT_PROBE_BED_POSITION 15
#define RIGHT_PROBE_BED_POSITION 170
#define BACK_PROBE_BED_POSITION 180
#define FRONT_PROBE_BED_POSITION 20
#endif
// set the number of grid points per dimension
// I wouldn't see a reason to go above 3 (=9 probing points on the bed)
#define AUTO_BED_LEVELING_GRID_POINTS 2
#else // not AUTO_BED_LEVELING_GRID
// with no grid, just probe 3 arbitrary points. A simple cross-product
// is used to esimate the plane of the print bed
#define ABL_PROBE_PT_1_X 15
#define ABL_PROBE_PT_1_Y 180
#define ABL_PROBE_PT_2_X 15
#define ABL_PROBE_PT_2_Y 20
#define ABL_PROBE_PT_3_X 170
#define ABL_PROBE_PT_3_Y 20
#endif // AUTO_BED_LEVELING_GRID
// these are the offsets to the probe relative to the extruder tip (Hotend - Probe)
#define X_PROBE_OFFSET_FROM_EXTRUDER -25
@ -418,19 +417,7 @@ 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
// Mode 2 only
#ifdef ACCURATE_BED_LEVELING
#ifdef AUTO_BED_LEVELING_ANY_POINTS
#error AUTO_BED_LEVELING_ANY_POINTS is incompatible with ACCURATE_BED_LEVELING
#endif
// I wouldn't see a reason to go above 3 (=9 probing points on the bed)
#define ACCURATE_BED_LEVELING_POINTS 2
#endif
#endif
#endif // ENABLE_AUTO_BED_LEVELING
// The position of the homing switches

90
Marlin/Marlin_main.cpp
View File

@ -31,7 +31,7 @@
#ifdef ENABLE_AUTO_BED_LEVELING
#include "vector_3.h"
#ifdef ACCURATE_BED_LEVELING
#ifdef AUTO_BED_LEVELING_GRID
#include "qr_solve.h"
#endif
#endif // ENABLE_AUTO_BED_LEVELING
@ -822,7 +822,7 @@ static void axis_is_at_home(int axis) {
}
#ifdef ENABLE_AUTO_BED_LEVELING
#ifdef ACCURATE_BED_LEVELING
#ifdef AUTO_BED_LEVELING_GRID
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);
@ -846,10 +846,9 @@ static void set_bed_level_equation_lsq(double *plane_equation_coefficients)
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
#else // not ACCURATE_BED_LEVELING
#else // not AUTO_BED_LEVELING_GRID
#ifdef AUTO_BED_LEVELING_ANY_POINTS
static void set_bed_level_equation_any_pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
plan_bed_level_matrix.set_to_identity();
@ -869,49 +868,14 @@ static void set_bed_level_equation_any_pts(float z_at_pt_1, float z_at_pt_2, flo
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.
// 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]);
}
#else // not AUTO_BED_LEVELING_ANY_POINTS
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();
vector_3 xLeftyFront = vector_3(LEFT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, z_at_xLeft_yFront);
vector_3 xLeftyBack = vector_3(LEFT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION, z_at_xLeft_yBack);
vector_3 xRightyFront = vector_3(RIGHT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, z_at_xRight_yFront);
vector_3 xPositive = (xRightyFront - xLeftyFront).get_normal();
vector_3 yPositive = (xLeftyBack - xLeftyFront).get_normal();
vector_3 planeNormal = vector_3::cross(xPositive, yPositive).get_normal();
//planeNormal.debug("planeNormal");
//yPositive.debug("yPositive");
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");
// and set our bed level equation to do the right thing
//plan_bed_level_matrix.debug("bed level after");
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] = zprobe_zoffset;
plan_set_position(current_position[X_AXIS], current_position[Y_AXIS], current_position[Z_AXIS], current_position[E_AXIS]);
}
#endif // AUTO_BED_LEVELING_ANY_POINTS
#endif // ACCURATE_BED_LEVELING
#endif // AUTO_BED_LEVELING_GRID
static void run_z_probe() {
plan_bed_level_matrix.set_to_identity();
@ -1462,11 +1426,11 @@ void process_commands()
setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS];
#ifdef ACCURATE_BED_LEVELING
#ifdef AUTO_BED_LEVELING_GRID
// probe at the points of a lattice grid
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);
int xGridSpacing = (RIGHT_PROBE_BED_POSITION - LEFT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
int yGridSpacing = (BACK_PROBE_BED_POSITION - FRONT_PROBE_BED_POSITION) / (AUTO_BED_LEVELING_GRID_POINTS-1);
// solve the plane equation ax + by + d = z
@ -1476,9 +1440,9 @@ void process_commands()
// 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];
double eqnAMatrix[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS*3];
// "B" vector of Z points
double eqnBVector[ACCURATE_BED_LEVELING_POINTS*ACCURATE_BED_LEVELING_POINTS];
double eqnBVector[AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS];
int probePointCounter = 0;
@ -1501,7 +1465,7 @@ void process_commands()
zig = true;
}
for (int xCount=0; xCount < ACCURATE_BED_LEVELING_POINTS; xCount++)
for (int xCount=0; xCount < AUTO_BED_LEVELING_GRID_POINTS; xCount++)
{
float z_before;
if (probePointCounter == 0)
@ -1518,9 +1482,9 @@ void process_commands()
eqnBVector[probePointCounter] = measured_z;
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;
eqnAMatrix[probePointCounter + 0*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = xProbe;
eqnAMatrix[probePointCounter + 1*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = yProbe;
eqnAMatrix[probePointCounter + 2*AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS] = 1;
probePointCounter++;
xProbe += xInc;
}
@ -1528,7 +1492,7 @@ void process_commands()
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);
double *plane_equation_coefficients = qr_solve(AUTO_BED_LEVELING_GRID_POINTS*AUTO_BED_LEVELING_GRID_POINTS, 3, eqnAMatrix, eqnBVector);
SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
SERIAL_PROTOCOL(plane_equation_coefficients[0]);
@ -1542,10 +1506,8 @@ void process_commands()
free(plane_equation_coefficients);
#else // ACCURATE_BED_LEVELING not defined
#else // AUTO_BED_LEVELING_GRID not defined
#ifdef AUTO_BED_LEVELING_ANY_POINTS
// Probe at 3 arbitrary points
// probe 1
float z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
@ -1558,24 +1520,10 @@ void process_commands()
clean_up_after_endstop_move();
set_bed_level_equation_any_pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
#else // not AUTO_BED_LEVELING_ANY_POINTS
// probe at 3 corners of a rectangle
// probe 1
float z_at_xLeft_yBack = probe_pt(LEFT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION, Z_RAISE_BEFORE_PROBING);
set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
// probe 2
float z_at_xLeft_yFront = probe_pt(LEFT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
// probe 3
float z_at_xRight_yFront = probe_pt(RIGHT_PROBE_BED_POSITION, FRONT_PROBE_BED_POSITION, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
clean_up_after_endstop_move();
set_bed_level_equation(z_at_xLeft_yFront, z_at_xRight_yFront, z_at_xLeft_yBack);
#endif
#endif // ACCURATE_BED_LEVELING
#endif // AUTO_BED_LEVELING_GRID
st_synchronize();
// The following code correct the Z height difference from z-probe position and hotend tip position.

2
Marlin/qr_solve.cpp
View File

@ -1,6 +1,6 @@
#include "qr_solve.h"
#ifdef ACCURATE_BED_LEVELING
#ifdef AUTO_BED_LEVELING_GRID
#include <stdlib.h>
#include <math.h>

2
Marlin/qr_solve.h
View File

@ -1,6 +1,6 @@
#include "Configuration.h"
#ifdef ACCURATE_BED_LEVELING
#ifdef AUTO_BED_LEVELING_GRID
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 );