Enhanced G29
- Adapted “Enhanced G29” code referred to in #1499 and posted at [3dprintboard.com](http://3dprintboard.com/showthread.php?3105-Auto_Bed_ Leveling-Enhanced-G29-command) - Compatible with current G29 while adding some new arguments - `V` sets the verbose level for serial out - `T` (or `V` > 2) send a Topology report to serial out - `E` works the same way as before - `P` works as before (source used `n` or `U`/`u`) - `L`, `R`, `B`, `F` work as before - Still needs sanity checking for `LRBF`
This commit is contained in:
Scott Lahteine
parent
e428e8da75
commit
7684721977
@ -1200,22 +1200,24 @@ static void retract_z_probe() {
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
|
enum ProbeAction { ProbeEngageRetract, ProbeEngage, ProbeStay, ProbeRetract };
|
||||||
|
|
||||||
/// Probe bed height at position (x,y), returns the measured z value
|
/// Probe bed height at position (x,y), returns the measured z value
|
||||||
static float probe_pt(float x, float y, float z_before, int retract_action=0) {
|
static float probe_pt(float x, float y, float z_before, ProbeAction retract_action=ProbeEngageRetract) {
|
||||||
// move to right place
|
// move to right place
|
||||||
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
|
do_blocking_move_to(current_position[X_AXIS], current_position[Y_AXIS], z_before);
|
||||||
do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
|
do_blocking_move_to(x - X_PROBE_OFFSET_FROM_EXTRUDER, y - Y_PROBE_OFFSET_FROM_EXTRUDER, current_position[Z_AXIS]);
|
||||||
|
|
||||||
#ifndef Z_PROBE_SLED
|
#ifndef Z_PROBE_SLED
|
||||||
if ((retract_action==0) || (retract_action==1))
|
if (retract_action == ProbeEngageRetract || retract_action == ProbeEngage) engage_z_probe();
|
||||||
engage_z_probe(); // Engage Z Servo endstop if available
|
#endif
|
||||||
#endif // Z_PROBE_SLED
|
|
||||||
run_z_probe();
|
run_z_probe();
|
||||||
float measured_z = current_position[Z_AXIS];
|
float measured_z = current_position[Z_AXIS];
|
||||||
#ifndef Z_PROBE_SLED
|
|
||||||
if ((retract_action==0) || (retract_action==3))
|
#ifndef Z_PROBE_SLED
|
||||||
retract_z_probe();
|
if (retract_action == ProbeEngageRetract || retract_action == ProbeRetract) retract_z_probe();
|
||||||
#endif // Z_PROBE_SLED
|
#endif
|
||||||
|
|
||||||
SERIAL_PROTOCOLPGM(MSG_BED);
|
SERIAL_PROTOCOLPGM(MSG_BED);
|
||||||
SERIAL_PROTOCOLPGM(" x: ");
|
SERIAL_PROTOCOLPGM(" x: ");
|
||||||
@ -1376,6 +1378,11 @@ void refresh_cmd_timeout(void)
|
|||||||
#endif //FWRETRACT
|
#endif //FWRETRACT
|
||||||
|
|
||||||
#ifdef Z_PROBE_SLED
|
#ifdef Z_PROBE_SLED
|
||||||
|
|
||||||
|
#ifndef SLED_DOCKING_OFFSET
|
||||||
|
#define SLED_DOCKING_OFFSET 0
|
||||||
|
#endif
|
||||||
|
|
||||||
//
|
//
|
||||||
// Method to dock/undock a sled designed by Charles Bell.
|
// Method to dock/undock a sled designed by Charles Bell.
|
||||||
//
|
//
|
||||||
@ -1719,193 +1726,286 @@ void process_commands()
|
|||||||
break;
|
break;
|
||||||
|
|
||||||
#ifdef ENABLE_AUTO_BED_LEVELING
|
#ifdef ENABLE_AUTO_BED_LEVELING
|
||||||
|
|
||||||
|
#if Z_MIN_PIN == -1
|
||||||
|
#error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling!!! Z_MIN_PIN must point to a valid hardware pin."
|
||||||
|
#endif
|
||||||
|
|
||||||
|
/**
|
||||||
|
* Enhanced G29 Auto Bed Leveling Probe Routine
|
||||||
|
*
|
||||||
|
* Parameters With AUTO_BED_LEVELING_GRID:
|
||||||
|
*
|
||||||
|
* P Set the size of the grid that will be probed (P x P points).
|
||||||
|
* Example: "G29 P4"
|
||||||
|
*
|
||||||
|
* V Set the verbose level (0-4). Example: "G29 V3"
|
||||||
|
*
|
||||||
|
* T Generate a Bed Topology Report. Example: "G29 P5 T" for a detailed report.
|
||||||
|
* This is useful for manual bed leveling and finding flaws in the bed (to
|
||||||
|
* assist with part placement).
|
||||||
|
*
|
||||||
|
* F Set the Front limit of the probing grid
|
||||||
|
* B Set the Back limit of the probing grid
|
||||||
|
* L Set the Left limit of the probing grid
|
||||||
|
* R Set the Right limit of the probing grid
|
||||||
|
*
|
||||||
|
* Global Parameters:
|
||||||
|
*
|
||||||
|
* E/e By default G29 engages / disengages the probe for each point.
|
||||||
|
* Include "E" to engage and disengage the probe just once.
|
||||||
|
* There's no extra effect if you have a fixed probe.
|
||||||
|
* Usage: "G29 E" or "G29 e"
|
||||||
|
*
|
||||||
|
*/
|
||||||
|
|
||||||
case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
|
case 29: // G29 Detailed Z-Probe, probes the bed at 3 or more points.
|
||||||
// Override probing area by providing [F]ront [B]ack [L]eft [R]ight Grid[P]oints values
|
{
|
||||||
{
|
// Use one of these defines to specify the origin
|
||||||
#if Z_MIN_PIN == -1
|
// for a topographical map to be printed for your bed.
|
||||||
#error "You must have a Z_MIN endstop in order to enable Auto Bed Leveling feature!!! Z_MIN_PIN must point to a valid hardware pin."
|
#define ORIGIN_BACK_LEFT 1
|
||||||
#endif
|
#define ORIGIN_FRONT_RIGHT 2
|
||||||
|
#define ORIGIN_BACK_RIGHT 3
|
||||||
|
#define ORIGIN_FRONT_LEFT 4
|
||||||
|
#define TOPO_ORIGIN ORIGIN_FRONT_LEFT
|
||||||
|
|
||||||
// Prevent user from running a G29 without first homing in X and Y
|
// Prevent user from running a G29 without first homing in X and Y
|
||||||
if (! (axis_known_position[X_AXIS] && axis_known_position[Y_AXIS]) )
|
if (!(axis_known_position[X_AXIS] && axis_known_position[Y_AXIS])) {
|
||||||
{
|
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
|
||||||
LCD_MESSAGEPGM(MSG_POSITION_UNKNOWN);
|
SERIAL_ECHO_START;
|
||||||
SERIAL_ECHO_START;
|
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
|
||||||
SERIAL_ECHOLNPGM(MSG_POSITION_UNKNOWN);
|
break; // abort G29, since we don't know where we are
|
||||||
break; // abort G29, since we don't know where we are
|
}
|
||||||
}
|
|
||||||
|
|
||||||
#ifdef Z_PROBE_SLED
|
bool enhanced_g29 = code_seen('E') || code_seen('e');
|
||||||
dock_sled(false);
|
|
||||||
#endif // Z_PROBE_SLED
|
|
||||||
st_synchronize();
|
|
||||||
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
|
|
||||||
//vector_3 corrected_position = plan_get_position_mm();
|
|
||||||
//corrected_position.debug("position before G29");
|
|
||||||
plan_bed_level_matrix.set_to_identity();
|
|
||||||
vector_3 uncorrected_position = plan_get_position();
|
|
||||||
//uncorrected_position.debug("position durring G29");
|
|
||||||
current_position[X_AXIS] = uncorrected_position.x;
|
|
||||||
current_position[Y_AXIS] = uncorrected_position.y;
|
|
||||||
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]);
|
|
||||||
setup_for_endstop_move();
|
|
||||||
|
|
||||||
feedrate = homing_feedrate[Z_AXIS];
|
#ifdef AUTO_BED_LEVELING_GRID
|
||||||
#ifdef AUTO_BED_LEVELING_GRID
|
|
||||||
// probe at the points of a lattice grid
|
|
||||||
int left_probe_bed_position=LEFT_PROBE_BED_POSITION;
|
|
||||||
int right_probe_bed_position=RIGHT_PROBE_BED_POSITION;
|
|
||||||
int back_probe_bed_position=BACK_PROBE_BED_POSITION;
|
|
||||||
int front_probe_bed_position=FRONT_PROBE_BED_POSITION;
|
|
||||||
int auto_bed_leveling_grid_points=AUTO_BED_LEVELING_GRID_POINTS;
|
|
||||||
if (code_seen('L')) left_probe_bed_position=(int)code_value();
|
|
||||||
if (code_seen('R')) right_probe_bed_position=(int)code_value();
|
|
||||||
if (code_seen('B')) back_probe_bed_position=(int)code_value();
|
|
||||||
if (code_seen('F')) front_probe_bed_position=(int)code_value();
|
|
||||||
if (code_seen('P')) auto_bed_leveling_grid_points=(int)code_value();
|
|
||||||
|
|
||||||
int xGridSpacing = (right_probe_bed_position - left_probe_bed_position) / (auto_bed_leveling_grid_points-1);
|
// Example Syntax: G29 N4 V2 E T
|
||||||
int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points-1);
|
int verbose_level = 1;
|
||||||
|
|
||||||
|
bool topo_flag = code_seen('T') || code_seen('t');
|
||||||
|
|
||||||
// solve the plane equation ax + by + d = z
|
if (code_seen('V') || code_seen('v')) {
|
||||||
// A is the matrix with rows [x y 1] for all the probed points
|
verbose_level = code_value();
|
||||||
// B is the vector of the Z positions
|
if (verbose_level < 0 || verbose_level > 4) {
|
||||||
// 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
|
SERIAL_PROTOCOLPGM("?Verbose Level not plausible (0-4).\n");
|
||||||
// so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
|
break;
|
||||||
|
}
|
||||||
// "A" matrix of the linear system of equations
|
if (verbose_level > 0) {
|
||||||
double eqnAMatrix[auto_bed_leveling_grid_points*auto_bed_leveling_grid_points*3];
|
SERIAL_PROTOCOLPGM("Enhanced G29 Auto_Bed_Leveling Code V1.25:\n");
|
||||||
|
SERIAL_PROTOCOLPGM("Full support at http://3dprintboard.com\n");
|
||||||
// "B" vector of Z points
|
if (verbose_level > 2) topo_flag = true;
|
||||||
double eqnBVector[auto_bed_leveling_grid_points*auto_bed_leveling_grid_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 < auto_bed_leveling_grid_points; xCount++)
|
|
||||||
{
|
|
||||||
float z_before;
|
|
||||||
if (probePointCounter == 0)
|
|
||||||
{
|
|
||||||
// raise before probing
|
|
||||||
z_before = Z_RAISE_BEFORE_PROBING;
|
|
||||||
} else
|
|
||||||
{
|
|
||||||
// raise extruder
|
|
||||||
z_before = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS;
|
|
||||||
}
|
|
||||||
|
|
||||||
float measured_z;
|
|
||||||
//Enhanced G29 - Do not retract servo between probes
|
|
||||||
if (code_seen('E') || code_seen('e') )
|
|
||||||
{
|
|
||||||
if ((yProbe==FRONT_PROBE_BED_POSITION) && (xCount==0))
|
|
||||||
{
|
|
||||||
measured_z = probe_pt(xProbe, yProbe, z_before,1);
|
|
||||||
} else if ((yProbe==FRONT_PROBE_BED_POSITION + (yGridSpacing * (AUTO_BED_LEVELING_GRID_POINTS-1))) && (xCount == AUTO_BED_LEVELING_GRID_POINTS-1))
|
|
||||||
{
|
|
||||||
measured_z = probe_pt(xProbe, yProbe, z_before,3);
|
|
||||||
} else {
|
|
||||||
measured_z = probe_pt(xProbe, yProbe, z_before,2);
|
|
||||||
}
|
|
||||||
} else {
|
|
||||||
measured_z = probe_pt(xProbe, yProbe, z_before);
|
|
||||||
}
|
|
||||||
|
|
||||||
eqnBVector[probePointCounter] = measured_z;
|
|
||||||
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
clean_up_after_endstop_move();
|
|
||||||
|
|
||||||
// solve lsq problem
|
|
||||||
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]);
|
|
||||||
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 // AUTO_BED_LEVELING_GRID not defined
|
|
||||||
|
|
||||||
// Probe at 3 arbitrary points
|
|
||||||
// Enhanced G29
|
|
||||||
|
|
||||||
float z_at_pt_1, z_at_pt_2, z_at_pt_3;
|
|
||||||
|
|
||||||
if (code_seen('E') || code_seen('e')) {
|
|
||||||
// probe 1
|
|
||||||
z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING,1);
|
|
||||||
// probe 2
|
|
||||||
z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,2);
|
|
||||||
// probe 3
|
|
||||||
z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,3);
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
// probe 1
|
|
||||||
z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
|
|
||||||
// probe 2
|
|
||||||
z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
|
||||||
// probe 3
|
|
||||||
z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
|
||||||
}
|
|
||||||
clean_up_after_endstop_move();
|
|
||||||
set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
|
|
||||||
|
|
||||||
|
|
||||||
#endif // AUTO_BED_LEVELING_GRID
|
|
||||||
st_synchronize();
|
|
||||||
|
|
||||||
// The following code 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.
|
|
||||||
// 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)
|
|
||||||
x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
|
|
||||||
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
|
|
||||||
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
|
|
||||||
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]);
|
|
||||||
#ifdef Z_PROBE_SLED
|
|
||||||
dock_sled(true, -SLED_DOCKING_OFFSET); // correct for over travel.
|
|
||||||
#endif // Z_PROBE_SLED
|
|
||||||
}
|
}
|
||||||
break;
|
|
||||||
|
int auto_bed_leveling_grid_points = code_seen('P') ? code_value_long() : AUTO_BED_LEVELING_GRID_POINTS;
|
||||||
|
if (auto_bed_leveling_grid_points < 2 || auto_bed_leveling_grid_points > AUTO_BED_LEVELING_GRID_POINTS) {
|
||||||
|
SERIAL_PROTOCOLPGM("?Number of probed points not plausible (2 minimum).\n");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
int left_probe_bed_position = code_seen('L') ? code_value_long() : LEFT_PROBE_BED_POSITION;
|
||||||
|
int right_probe_bed_position = code_seen('R') ? code_value_long() : RIGHT_PROBE_BED_POSITION;
|
||||||
|
int back_probe_bed_position = code_seen('B') ? code_value_long() : BACK_PROBE_BED_POSITION;
|
||||||
|
int front_probe_bed_position = code_seen('F') ? code_value_long() : FRONT_PROBE_BED_POSITION;
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifdef Z_PROBE_SLED
|
||||||
|
dock_sled(false); // engage (un-dock) the probe
|
||||||
|
#endif
|
||||||
|
|
||||||
|
st_synchronize();
|
||||||
|
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly
|
||||||
|
//vector_3 corrected_position = plan_get_position_mm();
|
||||||
|
//corrected_position.debug("position before G29");
|
||||||
|
plan_bed_level_matrix.set_to_identity();
|
||||||
|
vector_3 uncorrected_position = plan_get_position();
|
||||||
|
//uncorrected_position.debug("position durring G29");
|
||||||
|
current_position[X_AXIS] = uncorrected_position.x;
|
||||||
|
current_position[Y_AXIS] = uncorrected_position.y;
|
||||||
|
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]);
|
||||||
|
setup_for_endstop_move();
|
||||||
|
|
||||||
|
feedrate = homing_feedrate[Z_AXIS];
|
||||||
|
|
||||||
|
#ifdef AUTO_BED_LEVELING_GRID
|
||||||
|
// probe at the points of a lattice grid
|
||||||
|
|
||||||
|
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
|
||||||
|
// 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;
|
||||||
|
|
||||||
|
double eqnAMatrix[abl2 * 3], // "A" matrix of the linear system of equations
|
||||||
|
eqnBVector[abl2], // "B" vector of Z points
|
||||||
|
mean = 0.0;
|
||||||
|
|
||||||
|
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, xInc = xGridSpacing;
|
||||||
|
else
|
||||||
|
xProbe = right_probe_bed_position, xInc = -xGridSpacing;
|
||||||
|
|
||||||
|
// If topo_flag is set then don't zig-zag. Just scan in one direction.
|
||||||
|
// This gets the probe points in more readable order.
|
||||||
|
if (!topo_flag) zig = !zig;
|
||||||
|
|
||||||
|
for (int xCount = 0; xCount < auto_bed_leveling_grid_points; xCount++) {
|
||||||
|
// raise extruder
|
||||||
|
float z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,
|
||||||
|
measured_z;
|
||||||
|
|
||||||
|
// Enhanced G29 - Do not retract servo between probes
|
||||||
|
ProbeAction act;
|
||||||
|
if (enhanced_g29) {
|
||||||
|
if (yProbe == front_probe_bed_position && xCount == 0)
|
||||||
|
act = ProbeEngage;
|
||||||
|
else if (yProbe == front_probe_bed_position + (yGridSpacing * (auto_bed_leveling_grid_points - 1)) && xCount == auto_bed_leveling_grid_points - 1)
|
||||||
|
act = ProbeRetract;
|
||||||
|
else
|
||||||
|
act = ProbeStay;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
act = ProbeEngageRetract;
|
||||||
|
|
||||||
|
measured_z = probe_pt(xProbe, yProbe, z_before, act);
|
||||||
|
|
||||||
|
mean += measured_z;
|
||||||
|
|
||||||
|
eqnBVector[probePointCounter] = measured_z;
|
||||||
|
eqnAMatrix[probePointCounter + 0 * abl2] = xProbe;
|
||||||
|
eqnAMatrix[probePointCounter + 1 * abl2] = yProbe;
|
||||||
|
eqnAMatrix[probePointCounter + 2 * abl2] = 1;
|
||||||
|
|
||||||
|
probePointCounter++;
|
||||||
|
xProbe += xInc;
|
||||||
|
|
||||||
|
} //xProbe
|
||||||
|
|
||||||
|
} //yProbe
|
||||||
|
|
||||||
|
clean_up_after_endstop_move();
|
||||||
|
|
||||||
|
// solve lsq problem
|
||||||
|
double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
|
||||||
|
|
||||||
|
mean /= abl2;
|
||||||
|
|
||||||
|
if (verbose_level) {
|
||||||
|
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]);
|
||||||
|
if (verbose_level > 2) {
|
||||||
|
SERIAL_PROTOCOLPGM("Mean of sampled points: ");
|
||||||
|
SERIAL_PROTOCOL_F(mean, 6);
|
||||||
|
SERIAL_PROTOCOLPGM(" \n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (topo_flag) {
|
||||||
|
|
||||||
|
int xx, yy;
|
||||||
|
|
||||||
|
SERIAL_PROTOCOLPGM(" \nBed Height Topography: \n");
|
||||||
|
#if TOPO_ORIGIN == ORIGIN_FRONT_LEFT
|
||||||
|
for (yy = auto_bed_leveling_grid_points - 1; yy >= 0; yy--)
|
||||||
|
#else
|
||||||
|
for (yy = 0; yy < auto_bed_leveling_grid_points; yy++)
|
||||||
|
#endif
|
||||||
|
{
|
||||||
|
#if TOPO_ORIGIN == ORIGIN_BACK_RIGHT
|
||||||
|
for (xx = auto_bed_leveling_grid_points - 1; xx >= 0; xx--)
|
||||||
|
#else
|
||||||
|
for (xx = 0; xx < auto_bed_leveling_grid_points; xx++)
|
||||||
|
#endif
|
||||||
|
{
|
||||||
|
int ind =
|
||||||
|
#if TOPO_ORIGIN == ORIGIN_BACK_RIGHT || TOPO_ORIGIN == ORIGIN_FRONT_LEFT
|
||||||
|
yy * auto_bed_leveling_grid_points + xx
|
||||||
|
#elif TOPO_ORIGIN == ORIGIN_BACK_LEFT
|
||||||
|
xx * auto_bed_leveling_grid_points + yy
|
||||||
|
#elif TOPO_ORIGIN == ORIGIN_FRONT_RIGHT
|
||||||
|
abl2 - xx * auto_bed_leveling_grid_points - yy - 1
|
||||||
|
#endif
|
||||||
|
;
|
||||||
|
float diff = eqnBVector[ind] - mean;
|
||||||
|
if (diff >= 0.0)
|
||||||
|
SERIAL_PROTOCOLPGM(" +"); // Watch column alignment in Pronterface
|
||||||
|
else
|
||||||
|
SERIAL_PROTOCOLPGM(" -");
|
||||||
|
SERIAL_PROTOCOL_F(diff, 5);
|
||||||
|
} // xx
|
||||||
|
SERIAL_PROTOCOLPGM("\n");
|
||||||
|
} // yy
|
||||||
|
SERIAL_PROTOCOLPGM("\n");
|
||||||
|
|
||||||
|
} //topo_flag
|
||||||
|
|
||||||
|
|
||||||
|
set_bed_level_equation_lsq(plane_equation_coefficients);
|
||||||
|
free(plane_equation_coefficients);
|
||||||
|
|
||||||
|
#else // !AUTO_BED_LEVELING_GRID
|
||||||
|
|
||||||
|
// Probe at 3 arbitrary points
|
||||||
|
float z_at_pt_1, z_at_pt_2, z_at_pt_3;
|
||||||
|
|
||||||
|
if (enhanced_g29) {
|
||||||
|
// Basic Enhanced G29
|
||||||
|
z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING, ProbeEngage);
|
||||||
|
z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, ProbeStay);
|
||||||
|
z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS, ProbeRetract);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING);
|
||||||
|
z_at_pt_2 = probe_pt(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
||||||
|
z_at_pt_3 = probe_pt(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS);
|
||||||
|
}
|
||||||
|
clean_up_after_endstop_move();
|
||||||
|
set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
|
||||||
|
|
||||||
|
#endif // !AUTO_BED_LEVELING_GRID
|
||||||
|
|
||||||
|
st_synchronize();
|
||||||
|
|
||||||
|
if (verbose_level > 0)
|
||||||
|
plan_bed_level_matrix.debug(" \n\nBed Level Correction Matrix:");
|
||||||
|
|
||||||
|
// The following code 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.
|
||||||
|
// 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)
|
||||||
|
x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER;
|
||||||
|
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER;
|
||||||
|
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
|
||||||
|
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]);
|
||||||
|
|
||||||
|
#ifdef Z_PROBE_SLED
|
||||||
|
dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
#ifndef Z_PROBE_SLED
|
#ifndef Z_PROBE_SLED
|
||||||
case 30: // G30 Single Z Probe
|
case 30: // G30 Single Z Probe
|
||||||
{
|
{
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user