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
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      Marlin/Conditionals_post.h
  2. 284
      Marlin/Marlin_main.cpp
  3. 119
      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
#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)
static void set_bed_level_equation_lsq(double* plane_equation_coefficients) {
//planner.bed_level_matrix.debug("bed level before");
#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
vector_3 planeNormal = vector_3(-plane_equation_coefficients[0], -plane_equation_coefficients[1], 1);
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 DISABLED(DELTA)
#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();
/**
* 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))
);
#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
matrix_3x3 inverse = matrix_3x3::transpose(planner.bed_level_matrix);
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();
//pos.debug("untilted_stepper_position offset");
//bed_level_matrix.debug("untilted_stepper_position");
//inverse.debug("in untilted_stepper_position");
if (planeNormal.z < 0) {
planeNormal.x = -planeNormal.x;
planeNormal.y = -planeNormal.y;
planeNormal.z = -planeNormal.z;
}
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);
pos.y = LOGICAL_Y_POSITION(pos.y + Y_TILT_FULCRUM);
pos.z = LOGICAL_Z_POSITION(pos.z);
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_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
if (!dryrun) {
stepper.synchronize();
#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
if (!dryrun) {
// 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
//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;
//
// Re-orient the current position without leveling
// based on where the steppers are positioned.
//
#if ENABLED(DELTA) || ENABLED(SCARA)
#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)
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);
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
) {
#endif
#if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_leveling(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.
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
/**

Write Preview
Loading…
Cancel
Save