zaubentrucker/muele-marlin
1
0
Fork 0
Code Issues Pull requests Actions Packages 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
parent 416821a71c 9248cfb0ad
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
#define Z_SAFE_HOMING_Y_POINT ((Y_MIN_POS + Y_MAX_POS) / 2)
#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
/**

284
Marlin/Marlin_main.cpp
View file

@ -458,45 +458,51 @@ static uint8_t target_extruder;
#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 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
float delta_radius = DELTA_RADIUS;
float 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);
float 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);
float delta_tower3_x = 0; // back middle tower
float delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3);
float delta_diagonal_rod = DELTA_DIAGONAL_ROD;
float 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;
float 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);
float 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);
float delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND;
float delta_clip_start_height = Z_MAX_POS;
float delta_radius = DELTA_RADIUS,
delta_tower1_x = -SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1), // front left tower
delta_tower1_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_1),
delta_tower2_x = SIN_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2), // front right tower
delta_tower2_y = -COS_60 * (delta_radius + DELTA_RADIUS_TRIM_TOWER_2),
delta_tower3_x = 0, // back middle tower
delta_tower3_y = (delta_radius + DELTA_RADIUS_TRIM_TOWER_3),
delta_diagonal_rod = DELTA_DIAGONAL_ROD,
delta_diagonal_rod_trim_tower_1 = DELTA_DIAGONAL_ROD_TRIM_TOWER_1,
delta_diagonal_rod_trim_tower_2 = DELTA_DIAGONAL_ROD_TRIM_TOWER_2,
delta_diagonal_rod_trim_tower_3 = DELTA_DIAGONAL_ROD_TRIM_TOWER_3,
delta_diagonal_rod_2_tower_1 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_1),
delta_diagonal_rod_2_tower_2 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_2),
delta_diagonal_rod_2_tower_3 = sq(delta_diagonal_rod + delta_diagonal_rod_trim_tower_3),
delta_segments_per_second = DELTA_SEGMENTS_PER_SECOND,
delta_clip_start_height = Z_MAX_POS;
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
int delta_grid_spacing[2] = { 0, 0 };
float bed_level[AUTO_BED_LEVELING_GRID_POINTS][AUTO_BED_LEVELING_GRID_POINTS];
#endif
float delta_safe_distance_from_top();
void set_cartesian_from_steppers();
#else
static bool home_all_axis = true;
#endif
#if ENABLED(SCARA)
float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND;
float delta[ABC];
float axis_scaling[ABC] = { 1, 1, 1 }; // Build size scaling, default to 1
float delta_segments_per_second = SCARA_SEGMENTS_PER_SECOND,
delta[ABC],
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
#if ENABLED(FILAMENT_WIDTH_SENSOR)
@ -2266,79 +2272,37 @@ static void clean_up_after_endstop_or_probe_move() {
#if ENABLED(AUTO_BED_LEVELING_FEATURE)
#if ENABLED(AUTO_BED_LEVELING_GRID)
#if DISABLED(DELTA)
#if DISABLED(DELTA)
/**
* 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))
);
static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
//planner.bed_level_matrix.debug("bed level before");
//pos.debug("untilted_stepper_position offset");
//bed_level_matrix.debug("untilted_stepper_position");
//inverse.debug("in untilted_stepper_position");
#if ENABLED(DEBUG_LEVELING_FEATURE)
planner.bed_level_matrix.set_to_identity();
if (DEBUGGING(LEVELING)) {
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
pos.apply_rotation(inverse);
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
planner.bed_level_matrix = matrix_3x3::create_look_at(planeNormal);
pos.x = LOGICAL_X_POSITION(pos.x + X_TILT_FULCRUM);
pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM);
pos.z = LOGICAL_Z_POSITION(pos.z);
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;
//pos.debug("after rotation and reorientation");
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) DEBUG_POS("<<< set_bed_level_equation_lsq", corrected_position);
#endif
SYNC_PLAN_POSITION_KINEMATIC();
}
#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();
return pos;
}
#endif // !AUTO_BED_LEVELING_GRID
#endif // !DELTA
#if ENABLED(DELTA)
@ -3626,41 +3590,41 @@ inline void gcode_G28() {
#endif // AUTO_BED_LEVELING_GRID
stepper.synchronize();
if (!dryrun) {
#if ENABLED(DEBUG_LEVELING_FEATURE) && DISABLED(DELTA)
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();
#if ENABLED(DELTA)
reset_bed_level();
#else //!DELTA
//
// Re-orient the current position without leveling
// based on where the steppers are positioned.
//
#if ENABLED(DELTA) || ENABLED(SCARA)
//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);
#if ENABLED(DELTA)
reset_bed_level();
#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
}
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();
@ -3766,7 +3730,20 @@ inline void gcode_G28() {
LOGICAL_Y_POSITION(ABL_PROBE_PT_3_Y),
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
@ -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
if (do_topography_map) {
@ -3851,6 +3833,7 @@ inline void gcode_G28() {
SERIAL_EOL;
} // yy
SERIAL_EOL;
if (verbose_level > 3) {
SERIAL_PROTOCOLLNPGM("\nCorrected Bed Height vs. Bed Topology:");
@ -3876,47 +3859,60 @@ inline void gcode_G28() {
SERIAL_EOL;
}
} //do_topography_map
#endif //!DELTA
#endif // AUTO_BED_LEVELING_GRID
#if DISABLED(DELTA)
if (verbose_level > 0)
planner.bed_level_matrix.debug("\n\nBed Level Correction Matrix:");
if (!dryrun) {
/**
* Correct the Z height difference from Z probe position and nozzle tip position.
* 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.
*/
float 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],
stepper_z = stepper.get_axis_position_mm(Z_AXIS); //get the real Z (since planner.adjusted_position is now correcting the plane)
//
// 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.
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));
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("> BEFORE apply_rotation_xyz > stepper_z = ", stepper_z);
SERIAL_ECHOLNPAIR(" ... z_tmp = ", z_tmp);
SERIAL_ECHOPAIR("BEFORE ROTATION ... x_dist:", x_dist);
SERIAL_ECHOPAIR("y_dist:", y_dist);
SERIAL_ECHOPAIR("z_real:", z_real);
}
#endif
// Apply the correction sending the Z probe offset
apply_rotation_xyz(planner.bed_level_matrix, x_tmp, y_tmp, z_tmp);
// 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);
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING))
SERIAL_ECHOLNPAIR("> AFTER apply_rotation_xyz > z_tmp = ", z_tmp);
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR("AFTER ROTATION ... x_dist:", x_dist);
SERIAL_ECHOPAIR("y_dist:", y_dist);
SERIAL_ECHOPAIR("z_real:", z_real);
}
#endif
// Adjust the current Z and send it to the planner.
current_position[Z_AXIS] += z_tmp - stepper_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);
SYNC_PLAN_POSITION_KINEMATIC();
#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 // !DELTA
#ifdef Z_PROBE_END_SCRIPT
@ -7850,15 +7846,15 @@ void ok_to_send() {
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_y - cartesian[Y_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_y - cartesian[Y_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_y - cartesian[Y_AXIS])
) + cartesian[Z_AXIS];
@ -7867,9 +7863,9 @@ void ok_to_send() {
SERIAL_ECHOPGM(" y="); SERIAL_ECHO(cartesian[Y_AXIS]);
SERIAL_ECHOPGM(" z="); SERIAL_ECHOLN(cartesian[Z_AXIS]);
SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[TOWER_1]);
SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[TOWER_2]);
SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[TOWER_3]);
SERIAL_ECHOPGM("delta a="); SERIAL_ECHO(delta[A_AXIS]);
SERIAL_ECHOPGM(" b="); SERIAL_ECHO(delta[B_AXIS]);
SERIAL_ECHOPGM(" c="); SERIAL_ECHOLN(delta[C_AXIS]);
*/
}
@ -7880,10 +7876,10 @@ void ok_to_send() {
LOGICAL_Z_POSITION(0)
};
inverse_kinematics(cartesian);
float distance = delta[TOWER_3];
float distance = delta[A_AXIS];
cartesian[Y_AXIS] = LOGICAL_Y_POSITION(DELTA_PRINTABLE_RADIUS);
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) {
@ -8014,7 +8010,7 @@ void set_current_from_steppers_for_axis(AxisEnum axis) {
set_cartesian_from_steppers();
current_position[axis] = LOGICAL_POSITION(cartesian_position[axis], axis);
#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;
#else
current_position[axis] = stepper.get_axis_position_mm(axis); // CORE handled transparently

119
Marlin/planner.cpp
View file

@ -521,6 +521,38 @@ void Planner::check_axes_activity() {
#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
*
@ -531,12 +563,14 @@ void Planner::check_axes_activity() {
* extruder - target extruder
*/
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
void Planner::buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder)
#else
void Planner::buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder)
#endif // AUTO_BED_LEVELING_FEATURE
{
void Planner::buffer_line(
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
float x, float y, float z
#else
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
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.
while (block_buffer_tail == next_buffer_head) idle();
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active())
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);
#if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_leveling(x, y, z);
#endif
// The target position of the tool in absolute steps
@ -1116,61 +1147,33 @@ void Planner::check_axes_activity() {
} // 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).
*
* 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(float x, float y, float z, const float& e)
#else
void Planner::set_position_mm(const float& x, const float& y, const float& z, const float& e)
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
{
#if ENABLED(MESH_BED_LEVELING)
void Planner::set_position_mm(
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
float x, float y, float z
#else
const float& x, const float& y, const float& z
#endif
, const float& e
) {
if (mbl.active())
z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
#if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_leveling(x, y, z);
#endif
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]),
nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]),
ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
stepper.set_position(nx, ny, nz, ne);
previous_nominal_speed = 0.0; // Resets planner junction speeds. Assumes start from rest.
apply_rotation_xyz(bed_level_matrix, x, y, z);
#endif
long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]),
ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]),
nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_AXIS]),
ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
stepper.set_position(nx, ny, nz, ne);
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).

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)
/**
* The corrected position, applying the bed level matrix
*/
static vector_3 adjusted_position();
#if ENABLED(MESH_BED_LEVELING)
static void apply_leveling(const float &x, const float &y, float &z);
#else
static void apply_leveling(float &x, float &y, float &z);
#endif
/**