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

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`
pull/1/head
Scott Lahteine 10 years ago
parent
e428e8da75
commit
7684721977
1 changed files with 268 additions and 168 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. 290
      Marlin/Marlin_main.cpp

290
Marlin/Marlin_main.cpp
View File

@ -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 #ifndef Z_PROBE_SLED
if ((retract_action==0) || (retract_action==3)) if (retract_action == ProbeEngageRetract || retract_action == ProbeRetract) retract_z_probe();
retract_z_probe(); #endif
#endif // Z_PROBE_SLED
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,25 +1726,96 @@ 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
{ {
#if Z_MIN_PIN == -1 // Use one of these defines to specify the origin
#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." // for a topographical map to be printed for your bed.
#endif #define ORIGIN_BACK_LEFT 1
#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
} }
bool enhanced_g29 = code_seen('E') || code_seen('e');
#ifdef AUTO_BED_LEVELING_GRID
// Example Syntax: G29 N4 V2 E T
int verbose_level = 1;
bool topo_flag = code_seen('T') || code_seen('t');
if (code_seen('V') || code_seen('v')) {
verbose_level = code_value();
if (verbose_level < 0 || verbose_level > 4) {
SERIAL_PROTOCOLPGM("?Verbose Level not plausible (0-4).\n");
break;
}
if (verbose_level > 0) {
SERIAL_PROTOCOLPGM("Enhanced G29 Auto_Bed_Leveling Code V1.25:\n");
SERIAL_PROTOCOLPGM("Full support at http://3dprintboard.com\n");
if (verbose_level > 2) topo_flag = true;
}
}
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 #ifdef Z_PROBE_SLED
dock_sled(false); dock_sled(false); // engage (un-dock) the probe
#endif // Z_PROBE_SLED #endif
st_synchronize(); st_synchronize();
// make sure the bed_level_rotation_matrix is identity or the planner will get it incorectly // 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();
@ -1752,144 +1830,164 @@ void process_commands()
setup_for_endstop_move(); setup_for_endstop_move();
feedrate = homing_feedrate[Z_AXIS]; feedrate = homing_feedrate[Z_AXIS];
#ifdef AUTO_BED_LEVELING_GRID #ifdef AUTO_BED_LEVELING_GRID
// probe at the points of a lattice 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); 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); int yGridSpacing = (back_probe_bed_position - front_probe_bed_position) / (auto_bed_leveling_grid_points - 1);
// 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
// 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 // 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 // so Vx = -a Vy = -b Vz = 1 (we want the vector facing towards positive Z
// "A" matrix of the linear system of equations int abl2 = auto_bed_leveling_grid_points * auto_bed_leveling_grid_points;
double eqnAMatrix[auto_bed_leveling_grid_points*auto_bed_leveling_grid_points*3];
// "B" vector of Z points
double eqnBVector[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; int probePointCounter = 0;
bool zig = true; bool zig = true;
for (int yProbe=front_probe_bed_position; yProbe <= back_probe_bed_position; yProbe += yGridSpacing) for (int yProbe = front_probe_bed_position; yProbe <= back_probe_bed_position; yProbe += yGridSpacing) {
{
int xProbe, xInc; int xProbe, xInc;
if (zig) if (zig)
{ xProbe = left_probe_bed_position, xInc = xGridSpacing;
xProbe = left_probe_bed_position; else
//xEnd = right_probe_bed_position; xProbe = right_probe_bed_position, xInc = -xGridSpacing;
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++) // If topo_flag is set then don't zig-zag. Just scan in one direction.
{ // This gets the probe points in more readable order.
float z_before; if (!topo_flag) zig = !zig;
if (probePointCounter == 0)
{ for (int xCount = 0; xCount < auto_bed_leveling_grid_points; xCount++) {
// raise before probing
z_before = Z_RAISE_BEFORE_PROBING;
} else
{
// raise extruder // raise extruder
z_before = current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS; float z_before = probePointCounter == 0 ? Z_RAISE_BEFORE_PROBING : current_position[Z_AXIS] + Z_RAISE_BETWEEN_PROBINGS,
} measured_z;
float measured_z;
// Enhanced G29 - Do not retract servo between probes // Enhanced G29 - Do not retract servo between probes
if (code_seen('E') || code_seen('e') ) ProbeAction act;
{ if (enhanced_g29) {
if ((yProbe==FRONT_PROBE_BED_POSITION) && (xCount==0)) if (yProbe == front_probe_bed_position && xCount == 0)
{ act = ProbeEngage;
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)
} else if ((yProbe==FRONT_PROBE_BED_POSITION + (yGridSpacing * (AUTO_BED_LEVELING_GRID_POINTS-1))) && (xCount == AUTO_BED_LEVELING_GRID_POINTS-1)) act = ProbeRetract;
{ else
measured_z = probe_pt(xProbe, yProbe, z_before,3); act = ProbeStay;
} else {
measured_z = probe_pt(xProbe, yProbe, z_before,2);
}
} else {
measured_z = probe_pt(xProbe, yProbe, z_before);
} }
else
act = ProbeEngageRetract;
measured_z = probe_pt(xProbe, yProbe, z_before, act);
mean += measured_z;
eqnBVector[probePointCounter] = measured_z; eqnBVector[probePointCounter] = measured_z;
eqnAMatrix[probePointCounter + 0 * abl2] = xProbe;
eqnAMatrix[probePointCounter + 1 * abl2] = yProbe;
eqnAMatrix[probePointCounter + 2 * abl2] = 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++; probePointCounter++;
xProbe += xInc; xProbe += xInc;
}
} } //xProbe
} //yProbe
clean_up_after_endstop_move(); clean_up_after_endstop_move();
// solve lsq problem // solve lsq problem
double *plane_equation_coefficients = qr_solve(auto_bed_leveling_grid_points*auto_bed_leveling_grid_points, 3, eqnAMatrix, eqnBVector); double *plane_equation_coefficients = qr_solve(abl2, 3, eqnAMatrix, eqnBVector);
mean /= abl2;
if (verbose_level) {
SERIAL_PROTOCOLPGM("Eqn coefficients: a: "); SERIAL_PROTOCOLPGM("Eqn coefficients: a: ");
SERIAL_PROTOCOL(plane_equation_coefficients[0]); SERIAL_PROTOCOL(plane_equation_coefficients[0]);
SERIAL_PROTOCOLPGM(" b: "); SERIAL_PROTOCOLPGM(" b: ");
SERIAL_PROTOCOL(plane_equation_coefficients[1]); SERIAL_PROTOCOL(plane_equation_coefficients[1]);
SERIAL_PROTOCOLPGM(" d: "); SERIAL_PROTOCOLPGM(" d: ");
SERIAL_PROTOCOLLN(plane_equation_coefficients[2]); 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);
set_bed_level_equation_lsq(plane_equation_coefficients);
free(plane_equation_coefficients); free(plane_equation_coefficients);
#else // AUTO_BED_LEVELING_GRID not defined #else // !AUTO_BED_LEVELING_GRID
// Probe at 3 arbitrary points // Probe at 3 arbitrary points
// Enhanced G29
float z_at_pt_1, z_at_pt_2, z_at_pt_3; float z_at_pt_1, z_at_pt_2, z_at_pt_3;
if (code_seen('E') || code_seen('e')) { if (enhanced_g29) {
// probe 1 // Basic Enhanced G29
z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING,1); z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING, ProbeEngage);
// 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, ProbeStay);
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); 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);
// 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 { else {
// probe 1
z_at_pt_1 = probe_pt(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, Z_RAISE_BEFORE_PROBING); 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); 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); 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(); clean_up_after_endstop_move();
set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3); set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3);
#endif // !AUTO_BED_LEVELING_GRID
#endif // AUTO_BED_LEVELING_GRID
st_synchronize(); 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 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. // 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.
@ -1901,11 +1999,13 @@ void process_commands()
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]);
#ifdef Z_PROBE_SLED #ifdef Z_PROBE_SLED
dock_sled(true, -SLED_DOCKING_OFFSET); // correct for over travel. dock_sled(true, -SLED_DOCKING_OFFSET); // dock the probe, correcting for over-travel
#endif // Z_PROBE_SLED #endif
} }
break; break;
#ifndef Z_PROBE_SLED #ifndef Z_PROBE_SLED
case 30: // G30 Single Z Probe case 30: // G30 Single Z Probe
{ {

Write Preview
Loading…
Cancel
Save