Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use simplified Z correction in G29 when possible #4781

Merged
merged 1 commit into from
Sep 16, 2016
Merged

Use simplified Z correction in G29 when possible #4781

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
82 changes: 46 additions & 36 deletions Marlin/Marlin_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3397,6 +3397,8 @@ inline void gcode_G28() {

bed_leveling_in_progress = true;

float xProbe, yProbe, measured_z = 0;

#if ENABLED(AUTO_BED_LEVELING_GRID)

// probe at the points of a lattice grid
Expand Down Expand Up @@ -3434,8 +3436,8 @@ inline void gcode_G28() {
bool zig = auto_bed_leveling_grid_points & 1; //always end at [RIGHT_PROBE_BED_POSITION, BACK_PROBE_BED_POSITION]

for (uint8_t yCount = 0; yCount < auto_bed_leveling_grid_points; yCount++) {
float yBase = front_probe_bed_position + yGridSpacing * yCount,
yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5));
float yBase = front_probe_bed_position + yGridSpacing * yCount;
yProbe = floor(yBase + (yBase < 0 ? 0 : 0.5));
int8_t xStart, xStop, xInc;

if (zig) {
Expand All @@ -3452,8 +3454,8 @@ inline void gcode_G28() {
zig = !zig;

for (int8_t xCount = xStart; xCount != xStop; xCount += xInc) {
float xBase = left_probe_bed_position + xGridSpacing * xCount,
xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));
float xBase = left_probe_bed_position + xGridSpacing * xCount;
xProbe = floor(xBase + (xBase < 0 ? 0 : 0.5));

#if ENABLED(DELTA)
// Avoid probing outside the round or hexagonal area of a delta printer
Expand Down Expand Up @@ -3497,12 +3499,12 @@ inline void gcode_G28() {
vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, 0)
};

for (uint8_t i = 0; i < 3; ++i)
points[i].z = probe_pt(
LOGICAL_X_POSITION(points[i].x),
LOGICAL_Y_POSITION(points[i].y),
stow_probe_after_each, verbose_level
);
for (uint8_t i = 0; i < 3; ++i) {
// Retain the last probe position
xProbe = LOGICAL_X_POSITION(points[i].x);
yProbe = LOGICAL_Y_POSITION(points[i].y);
measured_z = points[i].z = probe_pt(xProbe, yProbe, stow_probe_after_each, verbose_level);
}

if (!dryrun) {
vector_3 planeNormal = vector_3::cross(points[0] - points[1], points[2] - points[1]).get_normal();
Expand Down Expand Up @@ -3635,42 +3637,50 @@ inline void gcode_G28() {
// Correct the current XYZ position based on the tilted plane.
//

// Get the distance from the reference point to the current position
// The current XY is in sync with the planner/steppers at this point
// but the current Z is only known to the steppers.
// 1. Get the distance from the current position to the reference point.
float x_dist = RAW_CURRENT_POSITION(X_AXIS) - X_TILT_FULCRUM,
y_dist = RAW_CURRENT_POSITION(Y_AXIS) - Y_TILT_FULCRUM,
z_real = RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS));
z_real = RAW_CURRENT_POSITION(Z_AXIS),
z_zero = 0;

#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("BEFORE ROTATION ... x_dist:", x_dist);
SERIAL_ECHOPAIR("y_dist:", y_dist);
SERIAL_ECHOPAIR("z_real:", z_real);
}
if (DEBUGGING(LEVELING)) DEBUG_POS("G29 uncorrected XYZ", current_position);
#endif

// Apply the matrix to the distance from the reference point to XY,
// and from the homed Z to the current Z.
apply_rotation_xyz(planner.bed_level_matrix, x_dist, y_dist, z_real);
matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);

#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("AFTER ROTATION ... x_dist:", x_dist);
SERIAL_ECHOPAIR("y_dist:", y_dist);
SERIAL_ECHOPAIR("z_real:", z_real);
}
#endif
// 2. Apply the inverse matrix to the distance
// from the reference point to X, Y, and zero.
apply_rotation_xyz(inverse, x_dist, y_dist, z_zero);

// 3. Get the matrix-based corrected Z.
// (Even if not used, get it for comparison.)
float new_z = z_real + z_zero;

// 4. Use the last measured distance to the bed, if possible
if ( NEAR(current_position[X_AXIS], xProbe - (X_PROBE_OFFSET_FROM_EXTRUDER))
&& NEAR(current_position[Y_AXIS], yProbe - (Y_PROBE_OFFSET_FROM_EXTRUDER))
) {
float simple_z = z_real - (measured_z - (-zprobe_zoffset));
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("Z from Probe:", simple_z);
SERIAL_ECHOPAIR(" Matrix:", new_z);
SERIAL_ECHOLNPAIR(" Discrepancy:", simple_z - new_z);
}
#endif
new_z = simple_z;
}

// Apply the rotated distance and Z to the current position
current_position[X_AXIS] = LOGICAL_X_POSITION(X_TILT_FULCRUM + x_dist);
current_position[Y_AXIS] = LOGICAL_Y_POSITION(Y_TILT_FULCRUM + y_dist);
current_position[Z_AXIS] = LOGICAL_Z_POSITION(z_real);
// 5. The rotated XY and corrected Z are now current_position
current_position[X_AXIS] = LOGICAL_X_POSITION(x_dist) + X_TILT_FULCRUM;
current_position[Y_AXIS] = LOGICAL_Y_POSITION(y_dist) + Y_TILT_FULCRUM;
current_position[Z_AXIS] = LOGICAL_Z_POSITION(new_z);

SYNC_PLAN_POSITION_KINEMATIC();

#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected XYZ in G29", current_position);
if (DEBUGGING(LEVELING)) DEBUG_POS("G29 corrected XYZ", current_position);
#endif
}

Expand Down Expand Up @@ -7962,8 +7972,8 @@ void set_current_from_steppers_for_axis(AxisEnum axis) {
LOOP_XYZE(i) difference[i] = target[i] - current_position[i];

float cartesian_mm = sqrt(sq(difference[X_AXIS]) + sq(difference[Y_AXIS]) + sq(difference[Z_AXIS]));
if (cartesian_mm < 0.000001) cartesian_mm = abs(difference[E_AXIS]);
if (cartesian_mm < 0.000001) return false;
if (UNEAR_ZERO(cartesian_mm)) cartesian_mm = abs(difference[E_AXIS]);
if (UNEAR_ZERO(cartesian_mm)) return false;
float _feedrate_mm_s = MMS_SCALED(feedrate_mm_s);
float seconds = cartesian_mm / _feedrate_mm_s;
int steps = max(1, int(delta_segments_per_second * seconds));
Expand Down