melch/Marlin
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Merge pull request #4710 from thinkyhead/rc_fix_leveling_maths

Browse Source 
Bed leveling that accounts for home XYZ
This commit is contained in:
Scott Lahteine 2016-09-10 15:47:39 -05:00 committed by GitHub
commit 48e14d049a
4 changed files with 210 additions and 207 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
Marlin/Conditionals_post.h
View File

@ -165,6 +165,11 @@
#ifndef Z_SAFE_HOMING_Y_POINT #ifndef Z_SAFE_HOMING_Y_POINT
#define Z_SAFE_HOMING_Y_POINT ((Y_MIN_POS + Y_MAX_POS) / 2) #define Z_SAFE_HOMING_Y_POINT ((Y_MIN_POS + Y_MAX_POS) / 2)
#endif #endif
#define X_TILT_FULCRUM Z_SAFE_HOMING_X_POINT
#define Y_TILT_FULCRUM Z_SAFE_HOMING_Y_POINT
#else
#define X_TILT_FULCRUM X_HOME_POS
#define Y_TILT_FULCRUM Y_HOME_POS
#endif #endif
/** /**

278
Marlin/Marlin_main.cpp
View File

@ -458,45 +458,51 @@ static uint8_t target_extruder;
#if ENABLED(DELTA) #if ENABLED(DELTA)
#define TOWER_1 X_AXIS
#define TOWER_2 Y_AXIS
#define TOWER_3 Z_AXIS
float delta[ABC];
float cartesian_position[XYZ] = { 0 };
#define SIN_60 0.8660254037844386 #define SIN_60 0.8660254037844386
#define COS_60 0.5 #define COS_60 0.5
float endstop_adj[ABC] = { 0 };
float delta[ABC],
cartesian_position[XYZ] = { 0 },
endstop_adj[ABC] = { 0 };
// these are the default values, can be overriden with M665 // these are the default values, can be overriden with M665
float delta_radius = DELTA_RADIUS; float delta_radius = DELTA_RADIUS,
float delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1); // front left tower delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), // front left tower
float delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1); delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1),
float delta_tower2_x = SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2); // front right tower delta_tower2_x = SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), // front right tower
float delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2); delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2),
float delta_tower3_x = 0; // back middle tower delta_tower3_x = 0, // back middle tower
float delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3); delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3),
float delta_diagonal_rod = DELTA_DIAGONAL_ROD; delta_diagonal_rod = DELTA_DIAGONAL_ROD,
float delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1; delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1,
float delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2; delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2,
float delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3; delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3,
float delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1); delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1),
float delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2); delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2),
float delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3); delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3),
float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND; delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND,
float delta_clip_start_height = Z_MAX_POS; delta_clip_start_height = Z_MAX_POS;
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
int delta_grid_spacing[2] = { 0, 0 }; int delta_grid_spacing[2] = { 0, 0 };
float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS]; float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
#endif #endif
float delta_safe_distance_from_top(); float delta_safe_distance_from_top();
void set_cartesian_from_steppers();
#else #else
static bool home_all_axis = true; static bool home_all_axis = true;
#endif #endif
#if ENABLED(SCARA) #if ENABLED(SCARA)
float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND; float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND,
float delta[ABC]; delta[ABC],
float axis_scaling[ABC] = { 1, 1, 1 }; // Build size scaling, default to 1 axis_scaling[ABC] = { 1, 1, 1 }, // Build size scaling, default to 1
cartesian_position[XYZ] = { 0 };
void set_cartesian_from_steppers() { } // to be written later
#endif #endif
#if ENABLED(FILAMENT_WIDTH_SENSOR) #if ENABLED(FILAMENT_WIDTH_SENSOR)
@ -2266,80 +2272,38 @@ static void clean_up_after_endstop_or_probe_move() {
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(AUTO_BED_LEVELING_FEATURE)
#if ENABLED(AUTO_BED_LEVELING_GRID)
#if DISABLED(DELTA) #if DISABLED(DELTA)
static void set_bed_level_equation_lsq(double* plane_equation_coefficients) { /**
* Get the stepper positions, apply the rotation matrix
* using the home XY and Z0 position as the fulcrum.
*/
vector_3 untilted_stepper_position() {
vector_3 pos = vector_3(
RAW_X_POSITION(stepper.get_axis_position_mm(X_AXIS)) - X_TILT_FULCRUM,
RAW_Y_POSITION(stepper.get_axis_position_mm(Y_AXIS)) - Y_TILT_FULCRUM,
RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS))
);
//planner.bed_level_matrix.debug("bed level before"); matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
#if ENABLED(DEBUG_LEVELING_FEATURE) //pos.debug("untilted_stepper_position offset");
planner.bed_level_matrix.set_to_identity(); //bed_level_matrix.debug("untilted_stepper_position");
if (DEBUGGING(LEVELING)) { //inverse.debug("in untilted_stepper_position");
vector_3 uncorrected_position = planner.adjusted_position();
DEBUG_POS(">>> set_bed_level_equation_lsq", uncorrected_position);
DEBUG_POS(">>> set_bed_level_equation_lsq", current_position);
}
#endif
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1); pos.apply_rotation(inverse);
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
vector_3 corrected_position = planner.adjusted_position(); pos.x = LOGICAL_X_POSITION(pos.x + X_TILT_FULCRUM);
current_position[X_AXIS] = corrected_position.x; pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM);
current_position[Y_AXIS] = corrected_position.y; pos.z = LOGICAL_Z_POSITION(pos.z);
current_position[Z_AXIS] = corrected_position.z;
#if ENABLED(DEBUG_LEVELING_FEATURE) //pos.debug("after rotation and reorientation");
if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position);
#endif
SYNC_PLAN_POSITION_KINEMATIC(); return pos;
} }
#endif // !DELTA #endif // !DELTA
#else // !AUTO_BED_LEVELING_GRID
static void set_bed_level_equation_3pts(float z_at_pt_1, float z_at_pt_2, float z_at_pt_3) {
planner.bed_level_matrix.set_to_identity();
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
vector_3 uncorrected_position = planner.adjusted_position();
DEBUG_POS("set_bed_level_equation_3pts", uncorrected_position);
}
#endif
vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1);
vector_3 pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2);
vector_3 pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
if (planeNormal.z < 0) {
planeNormal.x = -planeNormal.x;
planeNormal.y = -planeNormal.y;
planeNormal.z = -planeNormal.z;
}
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
vector_3 corrected_position = planner.adjusted_position();
current_position[X_AXIS] = corrected_position.x;
current_position[Y_AXIS] = corrected_position.y;
current_position[Z_AXIS] = corrected_position.z;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("set_bed_level_equation_3pts", corrected_position);
#endif
SYNC_PLAN_POSITION_KINEMATIC();
}
#endif // !AUTO_BED_LEVELING_GRID
#if ENABLED(DELTA) #if ENABLED(DELTA)
/** /**
@ -3626,41 +3590,41 @@ inline void gcode_G28() {
#endif // AUTO_BED_LEVELING_GRID #endif // AUTO_BED_LEVELING_GRID
stepper.synchronize();
if (!dryrun) { if (!dryrun) {
#if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(DELTA) // Reset the bed_level_matrix because leveling
if (DEBUGGING(LEVELING)) { // needs to be done without leveling enabled.
vector_3 corrected_position = planner.adjusted_position();
DEBUG_POS("BEFORE matrix.set_to_identity", corrected_position);
DEBUG_POS("BEFORE matrix.set_to_identity", current_position);
}
#endif
// make sure the bed_level_rotation_matrix is identity or the planner will get it wrong
planner.bed_level_matrix.set_to_identity(); planner.bed_level_matrix.set_to_identity();
//
// Re-orient the current position without leveling
// based on where the steppers are positioned.
//
#if ENABLED(DELTA) || ENABLED(SCARA)
#if ENABLED(DELTA) #if ENABLED(DELTA)
reset_bed_level(); reset_bed_level();
#else //!DELTA
//vector_3 corrected_position = planner.adjusted_position();
//corrected_position.debug("position before G29");
vector_3 uncorrected_position = planner.adjusted_position();
//uncorrected_position.debug("position during G29");
current_position[X_AXIS] = uncorrected_position.x;
current_position[Y_AXIS] = uncorrected_position.y;
current_position[Z_AXIS] = uncorrected_position.z;
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", uncorrected_position);
#endif #endif
SYNC_PLAN_POSITION_KINEMATIC(); // For DELTA/SCARA we need to apply forward kinematics.
// This returns raw positions and we remap to the space.
set_cartesian_from_steppers();
LOOP_XYZ(i) current_position[i] = LOGICAL_POSITION(cartesian_position[i], i);
#else
// For cartesian/core the steppers are already mapped to
// the coordinate space by design.
LOOP_XYZ(i) current_position[i] = stepper.get_axis_position_mm((AxisEnum)i);
#endif // !DELTA #endif // !DELTA
}
stepper.synchronize(); // Inform the planner about the new coordinates
// (This is probably not needed here)
SYNC_PLAN_POSITION_KINEMATIC();
}
setup_for_endstop_or_probe_move(); setup_for_endstop_or_probe_move();
@ -3766,7 +3730,20 @@ inline void gcode_G28() {
LOGICAL_Y_POSITION(ABL_PROBE_PT_3_Y), LOGICAL_Y_POSITION(ABL_PROBE_PT_3_Y),
stow_probe_after_each, verbose_level); stow_probe_after_each, verbose_level);
if (!dryrun) set_bed_level_equation_3pts(z_at_pt_1, z_at_pt_2, z_at_pt_3); if (!dryrun) {
vector_3 pt1 = vector_3(ABL_PROBE_PT_1_X, ABL_PROBE_PT_1_Y, z_at_pt_1),
pt2 = vector_3(ABL_PROBE_PT_2_X, ABL_PROBE_PT_2_Y, z_at_pt_2),
pt3 = vector_3(ABL_PROBE_PT_3_X, ABL_PROBE_PT_3_Y, z_at_pt_3);
vector_3 planeNormal = vector_3::cross(pt1 - pt2, pt3 - pt2).get_normal();
if (planeNormal.z < 0) {
planeNormal.x *= -1;
planeNormal.y *= -1;
planeNormal.z *= -1;
}
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
}
#endif // !AUTO_BED_LEVELING_GRID #endif // !AUTO_BED_LEVELING_GRID
@ -3810,7 +3787,12 @@ inline void gcode_G28() {
} }
} }
if (!dryrun) set_bed_level_equation_lsq(plane_equation_coefficients); // Create the matrix but don't correct the position yet
if (!dryrun) {
planner.bed_level_matrix = matrix_3x3::create_look_at(
vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1)
);
}
// Show the Topography map if enabled // Show the Topography map if enabled
if (do_topography_map) { if (do_topography_map) {
@ -3851,6 +3833,7 @@ inline void gcode_G28() {
SERIAL_EOL; SERIAL_EOL;
} // yy } // yy
SERIAL_EOL; SERIAL_EOL;
if (verbose_level > 3) { if (verbose_level > 3) {
SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:"); SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
@ -3876,47 +3859,60 @@ inline void gcode_G28() {
SERIAL_EOL; SERIAL_EOL;
} }
} //do_topography_map } //do_topography_map
#endif //!DELTA #endif //!DELTA
#endif // AUTO_BED_LEVELING_GRID #endif // AUTO_BED_LEVELING_GRID
#if DISABLED(DELTA) #if DISABLED(DELTA)
if (verbose_level > 0) if (verbose_level > 0)
planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:"); planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:");
if (!dryrun) { if (!dryrun) {
/** //
* Correct the Z height difference from Z probe position and nozzle tip position. // Correct the current XYZ position based on the tilted plane.
* The Z height on homing is measured by Z probe, but the Z probe is quite far //
* from the nozzle. When the bed is uneven, this height must be corrected.
*/ // Get the distance from the reference point to the current position
float x_tmp = current_position[X_AXIS] + X_PROBE_OFFSET_FROM_EXTRUDER, // The current XY is in sync with the planner/steppers at this point
y_tmp = current_position[Y_AXIS] + Y_PROBE_OFFSET_FROM_EXTRUDER, // but the current Z is only known to the steppers.
z_tmp = current_position[Z_AXIS], float x_dist = RAW_CURRENT_POSITION(X_AXIS) - X_TILT_FULCRUM,
stepper_z = stepper.get_axis_position_mm(Z_AXIS); //get the real Z (since planner.adjusted_position is now correcting the plane) y_dist = RAW_CURRENT_POSITION(Y_AXIS) - Y_TILT_FULCRUM,
z_real = RAW_Z_POSITION(stepper.get_axis_position_mm(Z_AXIS));
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) { if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("> BEFORE apply_rotation_xyz > stepper_z = ", stepper_z); SERIAL_ECHOPAIR("BEFORE ROTATION ... x_dist:", x_dist);
SERIAL_ECHOLNPAIR(" ... z_tmp = ", z_tmp); SERIAL_ECHOPAIR("y_dist:", y_dist);
SERIAL_ECHOPAIR("z_real:", z_real);
} }
#endif #endif
// Apply the correction sending the Z probe offset // Apply the matrix to the distance from the reference point to XY,
apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp); // and from the homed Z to the current Z.
apply_rotation_xyz(planner.bed_level_matrix, x_dist, y_dist, z_real);
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) if (DEBUGGING(LEVELING)) {
SERIAL_ECHOLNPAIR("> AFTER apply_rotation_xyz > z_tmp = ", z_tmp); SERIAL_ECHOPAIR("AFTER ROTATION ... x_dist:", x_dist);
SERIAL_ECHOPAIR("y_dist:", y_dist);
SERIAL_ECHOPAIR("z_real:", z_real);
}
#endif #endif
// Adjust the current Z and send it to the planner. // Apply the rotated distance and Z to the current position
current_position[Z_AXIS] += z_tmp - stepper_z; 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);
SYNC_PLAN_POSITION_KINEMATIC(); SYNC_PLAN_POSITION_KINEMATIC();
#if ENABLED(DEBUG_LEVELING_FEATURE) #if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected Z in G29", current_position); if (DEBUGGING(LEVELING)) DEBUG_POS("> corrected XYZ in G29", current_position);
#endif #endif
} }
#endif // !DELTA #endif // !DELTA
#ifdef Z_PROBE_END_SCRIPT #ifdef Z_PROBE_END_SCRIPT
@ -7850,15 +7846,15 @@ void ok_to_send() {
RAW_Z_POSITION(in_cartesian[Z_AXIS]) RAW_Z_POSITION(in_cartesian[Z_AXIS])
}; };
delta[TOWER_1] = sqrt(delta_diagonal_rod_2_tower_1 delta[A_AXIS] = sqrt(delta_diagonal_rod_2_tower_1
- sq(delta_tower1_x - cartesian[X_AXIS]) - sq(delta_tower1_x - cartesian[X_AXIS])
- sq(delta_tower1_y - cartesian[Y_AXIS]) - sq(delta_tower1_y - cartesian[Y_AXIS])
) + cartesian[Z_AXIS]; ) + cartesian[Z_AXIS];
delta[TOWER_2] = sqrt(delta_diagonal_rod_2_tower_2 delta[B_AXIS] = sqrt(delta_diagonal_rod_2_tower_2
- sq(delta_tower2_x - cartesian[X_AXIS]) - sq(delta_tower2_x - cartesian[X_AXIS])
- sq(delta_tower2_y - cartesian[Y_AXIS]) - sq(delta_tower2_y - cartesian[Y_AXIS])
) + cartesian[Z_AXIS]; ) + cartesian[Z_AXIS];
delta[TOWER_3] = sqrt(delta_diagonal_rod_2_tower_3 delta[C_AXIS] = sqrt(delta_diagonal_rod_2_tower_3
- sq(delta_tower3_x - cartesian[X_AXIS]) - sq(delta_tower3_x - cartesian[X_AXIS])
- sq(delta_tower3_y - cartesian[Y_AXIS]) - sq(delta_tower3_y - cartesian[Y_AXIS])
) + cartesian[Z_AXIS]; ) + cartesian[Z_AXIS];
@ -7867,9 +7863,9 @@ void ok_to_send() {
SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]); SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]); SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[TOWER_1]); SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[A_AXIS]);
SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[TOWER_2]); SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[B_AXIS]);
SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[TOWER_3]); SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[C_AXIS]);
*/ */
} }
@ -7880,10 +7876,10 @@ void ok_to_send() {
LOGICAL_Z_POSITION(0) LOGICAL_Z_POSITION(0)
}; };
inverse_kinematics(cartesian); inverse_kinematics(cartesian);
float distance = delta[TOWER_3]; float distance = delta[A_AXIS];
cartesian[Y_AXIS] = LOGICAL_Y_POSITION(DELTA_PRINTABLE_RADIUS); cartesian[Y_AXIS] = LOGICAL_Y_POSITION(DELTA_PRINTABLE_RADIUS);
inverse_kinematics(cartesian); inverse_kinematics(cartesian);
return abs(distance - delta[TOWER_3]); return abs(distance - delta[A_AXIS]);
} }
void forward_kinematics_DELTA(float z1, float z2, float z3) { void forward_kinematics_DELTA(float z1, float z2, float z3) {
@ -8014,7 +8010,7 @@ void set_current_from_steppers_for_axis(AxisEnum axis) {
set_cartesian_from_steppers(); set_cartesian_from_steppers();
current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis); current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE) #elif ENABLED(AUTO_BED_LEVELING_FEATURE)
vector_3 pos = planner.adjusted_position(); vector_3 pos = untilted_stepper_position();
current_position[axis] = axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z; current_position[axis] = axis == X_AXIS ? pos.x : axis == Y_AXIS ? pos.y : pos.z;
#else #else
current_position[axis] = stepper.get_axis_position_mm(axis); // CORE handled transparently current_position[axis] = stepper.get_axis_position_mm(axis); // CORE handled transparently

103
Marlin/planner.cpp
View File

@ -521,6 +521,38 @@ void Planner::check_axes_activity() {
#endif #endif
} }
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
void Planner::apply_leveling(
#if ENABLED(MESH_BED_LEVELING)
const float &x, const float &y
#else
float &x, float &y
#endif
, float &z
) {
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active())
z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM),
ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM),
tz = RAW_Z_POSITION(z);
apply_rotation_xyz(bed_level_matrix, tx, ty, tz);
x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM);
y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM);
z = LOGICAL_Z_POSITION(tz);
#endif
}
#endif
/** /**
* Planner::buffer_line * Planner::buffer_line
* *
@ -531,12 +563,14 @@ void Planner::check_axes_activity() {
* extruder - target extruder * extruder - target extruder
*/ */
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) void Planner::buffer_line(
void Planner::buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder) #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
#else float x, float y, float z
void Planner::buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder) #else
#endif // AUTO_BED_LEVELING_FEATURE const float& x, const float& y, const float& z
{ #endif
, const float& e, float fr_mm_s, const uint8_t extruder
) {
// Calculate the buffer head after we push this byte // Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head); int next_buffer_head = next_block_index(block_buffer_head);
@ -544,11 +578,8 @@ void Planner::check_axes_activity() {
// Rest here until there is room in the buffer. // Rest here until there is room in the buffer.
while (block_buffer_tail == next_buffer_head) idle(); while (block_buffer_tail == next_buffer_head) idle();
#if ENABLED(MESH_BED_LEVELING) #if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
if (mbl.active()) apply_leveling(x, y, z);
z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(bed_level_matrix, x, y, z);
#endif #endif
// The target position of the tool in absolute steps // The target position of the tool in absolute steps
@ -1116,50 +1147,22 @@ void Planner::check_axes_activity() {
} // buffer_line() } // buffer_line()
#if ENABLED(AUTO_BED_LEVELING_FEATURE) && DISABLED(DELTA)
/**
* Get the XYZ position of the steppers as a vector_3.
*
* On CORE machines XYZ is derived from ABC.
*/
vector_3 Planner::adjusted_position() {
vector_3 pos = vector_3(stepper.get_axis_position_mm(X_AXIS), stepper.get_axis_position_mm(Y_AXIS), stepper.get_axis_position_mm(Z_AXIS));
//pos.debug("in Planner::adjusted_position");
//bed_level_matrix.debug("in Planner::adjusted_position");
matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
//inverse.debug("in Planner::inverse");
pos.apply_rotation(inverse);
//pos.debug("after rotation");
return pos;
}
#endif // AUTO_BED_LEVELING_FEATURE && !DELTA
/** /**
* Directly set the planner XYZ position (hence the stepper positions). * Directly set the planner XYZ position (hence the stepper positions).
* *
* On CORE machines stepper ABC will be translated from the given XYZ. * On CORE machines stepper ABC will be translated from the given XYZ.
*/ */
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) void Planner::set_position_mm(
void Planner::set_position_mm(float x, float y, float z, const float& e) #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
#else float x, float y, float z
void Planner::set_position_mm(const float& x, const float& y, const float& z, const float& e) #else
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING const float& x, const float& y, const float& z
{ #endif
#if ENABLED(MESH_BED_LEVELING) , const float& e
) {
if (mbl.active())
z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(bed_level_matrix, x, y, z);
#if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_leveling(x, y, z);
#endif #endif
long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]), long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
@ -1170,7 +1173,7 @@ void Planner::check_axes_activity() {
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest. previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
LOOP_XYZE(i) previous_speed[i] = 0.0; LOOP_XYZE(i) previous_speed[i] = 0.0;
} }
/** /**
* Directly set the planner E position (hence the stepper E position). * Directly set the planner E position (hence the stepper E position).

9
Marlin/planner.h
View File

@ -203,11 +203,10 @@ class Planner {
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING) #if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
#if ENABLED(AUTO_BED_LEVELING_FEATURE) #if ENABLED(MESH_BED_LEVELING)
/** static void apply_leveling(const float &x, const float &y, float &z);
* The corrected position, applying the bed level matrix #else
*/ static void apply_leveling(float &x, float &y, float &z);
static vector_3 adjusted_position();
#endif #endif
/** /**