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

Merge pull request #4710 from thinkyhead/rc_fix_leveling_maths 

Bed leveling that accounts for home XYZ
pull/1/head
Scott Lahteine 8 years ago
committed by GitHub
parent
416821a71c 9248cfb0ad
commit
48e14d049a
4 changed files with 210 additions and 207 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. 5
      Marlin/Conditionals_post.h
  2. 276
      Marlin/Marlin_main.cpp
  3. 103
      Marlin/planner.cpp
  4. 9
      Marlin/planner.h

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
/** /**

276
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
if (!dryrun) { stepper.synchronize();
#if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(DELTA) if (!dryrun) {
if (DEBUGGING(LEVELING)) {
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 // Reset the bed_level_matrix because leveling
// needs to be done without leveling enabled.
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 #endif
//vector_3 corrected_position = planner.adjusted_position(); // For DELTA/SCARA we need to apply forward kinematics.
//corrected_position.debug("position before G29"); // This returns raw positions and we remap to the space.
vector_3 uncorrected_position = planner.adjusted_position(); set_cartesian_from_steppers();
//uncorrected_position.debug("position during G29"); LOOP_XYZ(i) current_position[i] = LOGICAL_POSITION(cartesian_position[i], i);
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) #else
if (DEBUGGING(LEVELING)) DEBUG_POS("AFTER matrix.set_to_identity", uncorrected_position);
#endif
SYNC_PLAN_POSITION_KINEMATIC(); // 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
/** /**

Write Preview
Loading…
Cancel
Save