MPCNC/Firmware
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fix planner leveling and rename arguments

Browse Source 
Use lx, ly, lz for “logical” positions
This commit is contained in:
Scott Lahteine 2016-09-11 21:40:44 -05:00
parent d4f21af6b3
commit c109399bf6
2 changed files with 96 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
Marlin/planner.cpp
View File

@ -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_ECHOPAIR(" (", dx);
SERIAL_ECHOPAIR(") Y:", y);
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);
SERIAL_ECHOPAIR(") Z:", z);
#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
@ -1124,7 +1141,7 @@ void Planner::buffer_line(
block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
}
/**
SERIAL_ECHO_START;
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
@ -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.

66
Marlin/planner.h
View File

@ -202,39 +202,45 @@ 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);
#else
static void apply_leveling(float &x, float &y, float &z);
#endif
/**
* Add a new linear movement to the buffer.
*
* x,y,z,e - target position in mm
* 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);
/**
* Set the planner.position and individual stepper positions.
* Used by G92, G28, G29, and other procedures.
*
* Multiplies by axis_steps_per_mm[] and does necessary conversion
* for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
*
* Clears previous speed values.
*/
static void set_position_mm(float x, float y, float z, const float& e);
#define ARG_X float lx
#define ARG_Y float ly
#define ARG_Z float lz
#else
#define ARG_X const float &lx
#define ARG_Y const float &ly
#define ARG_Z const float &lz
#endif
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);
#if PLANNER_LEVELING
#endif // AUTO_BED_LEVELING_FEATURE || MESH_BED_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
/**
* Add a new linear movement to the buffer.
*
* x,y,z,e - target position in mm
* fr_mm_s - (target) speed of the move (mm/s)
* extruder - target 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.
* Used by G92, G28, G29, and other procedures.
*
* Multiplies by axis_steps_per_mm[] and does necessary conversion
* for COREXY / COREXZ / COREYZ to set the corresponding stepper positions.
*
* Clears previous speed values.
*/
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)