Fix planner leveling and rename arguments
Use lx, ly, lz for “logical” positions
This commit is contained in:
Scott Lahteine
parent
d4f21af6b3
commit
c109399bf6
2 changed files with 96 additions and 80 deletions
|
|
@ -523,30 +523,44 @@ void Planner::check_axes_activity() {
|
|||
|
||||
#if PLANNER_LEVELING
|
||||
|
||||
void Planner::apply_leveling(
|
||||
#if ENABLED(MESH_BED_LEVELING)
|
||||
const float &x, const float &y
|
||||
#else
|
||||
float &x, float &y
|
||||
#endif
|
||||
, float &z
|
||||
) {
|
||||
void Planner::apply_leveling(float &lx, float &ly, float &lz) {
|
||||
#if ENABLED(MESH_BED_LEVELING)
|
||||
|
||||
if (mbl.active())
|
||||
z += mbl.get_z(RAW_X_POSITION(x), RAW_Y_POSITION(y));
|
||||
lz += mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
|
||||
|
||||
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
|
||||
#elif ENABLED(AUTO_BED_LEVELING_LINEAR)
|
||||
|
||||
float tx = RAW_X_POSITION(x) - (X_TILT_FULCRUM),
|
||||
ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM),
|
||||
tz = RAW_Z_POSITION(z);
|
||||
float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM),
|
||||
dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM),
|
||||
dz = RAW_Z_POSITION(lz);
|
||||
|
||||
apply_rotation_xyz(bed_level_matrix, tx, ty, tz);
|
||||
apply_rotation_xyz(bed_level_matrix, dx, dy, dz);
|
||||
|
||||
x = LOGICAL_X_POSITION(tx + X_TILT_FULCRUM);
|
||||
y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM);
|
||||
z = LOGICAL_Z_POSITION(tz);
|
||||
lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
|
||||
ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
|
||||
lz = LOGICAL_Z_POSITION(dz);
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void Planner::unapply_leveling(float &lx, float &ly, float &lz) {
|
||||
#if ENABLED(MESH_BED_LEVELING)
|
||||
|
||||
if (mbl.active())
|
||||
lz -= mbl.get_z(RAW_X_POSITION(lx), RAW_Y_POSITION(ly));
|
||||
|
||||
#elif ENABLED(AUTO_BED_LEVELING_LINEAR)
|
||||
|
||||
matrix_3x3 inverse = matrix_3x3::transpose(bed_level_matrix);
|
||||
|
||||
float dx = lx - (X_TILT_FULCRUM), dy = ly - (Y_TILT_FULCRUM), dz = lz;
|
||||
|
||||
apply_rotation_xyz(inverse, dx, dy, dz);
|
||||
|
||||
lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
|
||||
ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
|
||||
lz = LOGICAL_Z_POSITION(dz);
|
||||
|
||||
#endif
|
||||
}
|
||||
|
|
@ -562,15 +576,7 @@ void Planner::check_axes_activity() {
|
|||
* fr_mm_s - (target) speed of the move
|
||||
* extruder - target extruder
|
||||
*/
|
||||
|
||||
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
|
||||
) {
|
||||
void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, 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);
|
||||
|
||||
|
|
@ -578,17 +584,17 @@ void Planner::buffer_line(
|
|||
// Rest here until there is room in the buffer.
|
||||
while (block_buffer_tail == next_buffer_head) idle();
|
||||
|
||||
#if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
|
||||
apply_leveling(x, y, z);
|
||||
#if PLANNER_LEVELING
|
||||
apply_leveling(lx, ly, lz);
|
||||
#endif
|
||||
|
||||
// The target position of the tool in absolute steps
|
||||
// Calculate target position in absolute steps
|
||||
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
|
||||
long target[NUM_AXIS] = {
|
||||
lround(x * axis_steps_per_mm[X_AXIS]),
|
||||
lround(y * axis_steps_per_mm[Y_AXIS]),
|
||||
lround(z * axis_steps_per_mm[Z_AXIS]),
|
||||
lround(lx * axis_steps_per_mm[X_AXIS]),
|
||||
lround(ly * axis_steps_per_mm[Y_AXIS]),
|
||||
lround(lz * axis_steps_per_mm[Z_AXIS]),
|
||||
lround(e * axis_steps_per_mm[E_AXIS])
|
||||
};
|
||||
|
||||
|
|
@ -598,11 +604,22 @@ void Planner::buffer_line(
|
|||
|
||||
/*
|
||||
SERIAL_ECHO_START;
|
||||
SERIAL_ECHOPAIR("Planner X:", x);
|
||||
SERIAL_ECHOPGM("Planner ", x);
|
||||
#if IS_KINEMATIC
|
||||
SERIAL_ECHOPAIR("A:", x);
|
||||
SERIAL_ECHOPAIR(" (", dx);
|
||||
SERIAL_ECHOPAIR(") B:", y);
|
||||
#else
|
||||
SERIAL_ECHOPAIR("X:", x);
|
||||
SERIAL_ECHOPAIR(" (", dx);
|
||||
SERIAL_ECHOPAIR(") Y:", y);
|
||||
#endif
|
||||
SERIAL_ECHOPAIR(" (", dy);
|
||||
#elif ENABLED(DELTA)
|
||||
SERIAL_ECHOPAIR(") C:", z);
|
||||
#else
|
||||
SERIAL_ECHOPAIR(") Z:", z);
|
||||
#endif
|
||||
SERIAL_ECHOPAIR(" (", dz);
|
||||
SERIAL_ECHOLNPGM(")");
|
||||
//*/
|
||||
|
|
@ -671,7 +688,7 @@ void Planner::buffer_line(
|
|||
// For a mixing extruder, get a magnified step_event_count for each
|
||||
#if ENABLED(MIXING_EXTRUDER)
|
||||
for (uint8_t i = 0; i < MIXING_STEPPERS; i++)
|
||||
block->mix_event_count[i] = (mixing_factor[i] < 0.0001) ? 0 : block->step_event_count / mixing_factor[i];
|
||||
block->mix_event_count[i] = UNEAR_ZERO(mixing_factor[i]) ? 0 : block->step_event_count / mixing_factor[i];
|
||||
#endif
|
||||
|
||||
#if FAN_COUNT > 0
|
||||
|
|
@ -1152,22 +1169,15 @@ void Planner::buffer_line(
|
|||
*
|
||||
* On CORE machines stepper ABC will be translated from the given XYZ.
|
||||
*/
|
||||
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
|
||||
) {
|
||||
void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
|
||||
|
||||
#if ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE)
|
||||
apply_leveling(x, y, z);
|
||||
#if PLANNER_LEVELING
|
||||
apply_leveling(lx, ly, lz);
|
||||
#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]),
|
||||
long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]),
|
||||
ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
|
||||
nz = position[Z_AXIS] = lround(lz * 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.
|
||||
|
|
|
|||
|
|
@ -202,11 +202,24 @@ class Planner {
|
|||
static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); }
|
||||
|
||||
#if ENABLED(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
|
||||
|
||||
#if ENABLED(MESH_BED_LEVELING)
|
||||
static void apply_leveling(const float &x, const float &y, float &z);
|
||||
#define ARG_X float lx
|
||||
#define ARG_Y float ly
|
||||
#define ARG_Z float lz
|
||||
#else
|
||||
static void apply_leveling(float &x, float &y, float &z);
|
||||
#define ARG_X const float &lx
|
||||
#define ARG_Y const float &ly
|
||||
#define ARG_Z const float &lz
|
||||
#endif
|
||||
|
||||
#if PLANNER_LEVELING
|
||||
|
||||
/**
|
||||
* Apply leveling to transform a cartesian position
|
||||
* as it will be given to the planner and steppers.
|
||||
*/
|
||||
static void apply_leveling(float &lx, float &ly, float &lz);
|
||||
static void unapply_leveling(float &lx, float &ly, float &lz);
|
||||
|
||||
#endif
|
||||
|
||||
/**
|
||||
|
|
@ -216,7 +229,7 @@ class Planner {
|
|||
* fr_mm_s - (target) speed of the move (mm/s)
|
||||
* extruder - target extruder
|
||||
*/
|
||||
static void buffer_line(float x, float y, float z, const float& e, float fr_mm_s, const uint8_t extruder);
|
||||
static void buffer_line(ARG_X, ARG_Y, ARG_Z, const float& e, float fr_mm_s, const uint8_t extruder);
|
||||
|
||||
/**
|
||||
* Set the planner.position and individual stepper positions.
|
||||
|
|
@ -227,14 +240,7 @@ class Planner {
|
|||
*
|
||||
* Clears previous speed values.
|
||||
*/
|
||||
static void set_position_mm(float x, float y, float z, const float& e);
|
||||
|
||||
#else
|
||||
|
||||
static void buffer_line(const float& x, const float& y, const float& z, const float& e, float fr_mm_s, const uint8_t extruder);
|
||||
static void set_position_mm(const float& x, const float& y, const float& z, const float& e);
|
||||
|
||||
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_LEVELING
|
||||
static void set_position_mm(ARG_X, ARG_Y, ARG_Z, const float& e);
|
||||
|
||||
/**
|
||||
* Set the E position (mm) of the planner (and the E stepper)
|
||||
|
|
|
|||
Reference in a new issue