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

102
Marlin/planner.cpp
View File

@ -523,30 +523,44 @@ void Planner::check_axes_activity() {
#if PLANNER_LEVELING #if PLANNER_LEVELING
void Planner::apply_leveling( void Planner::apply_leveling(float &lx, float &ly, float &lz) {
#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 ENABLED(MESH_BED_LEVELING)
if (mbl.active()) 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), float dx = RAW_X_POSITION(lx) - (X_TILT_FULCRUM),
ty = RAW_Y_POSITION(y) - (Y_TILT_FULCRUM), dy = RAW_Y_POSITION(ly) - (Y_TILT_FULCRUM),
tz = RAW_Z_POSITION(z); 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); lx = LOGICAL_X_POSITION(dx + X_TILT_FULCRUM);
y = LOGICAL_Y_POSITION(ty + Y_TILT_FULCRUM); ly = LOGICAL_Y_POSITION(dy + Y_TILT_FULCRUM);
z = LOGICAL_Z_POSITION(tz); 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 #endif
} }
@ -562,15 +576,7 @@ void Planner::check_axes_activity() {
* fr_mm_s - (target) speed of the move * fr_mm_s - (target) speed of the move
* extruder - target extruder * extruder - target extruder
*/ */
void Planner::buffer_line(ARG_X, ARG_Y, ARG_Z, const float &e, float fr_mm_s, const uint8_t 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
) {
// 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);
@ -578,17 +584,17 @@ void Planner::buffer_line(
// 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) || ENABLED(AUTO_BED_LEVELING_FEATURE) #if PLANNER_LEVELING
apply_leveling(x, y, z); apply_leveling(lx, ly, lz);
#endif #endif
// The target position of the tool in absolute steps // The target position of the tool in absolute steps
// Calculate target position 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 //this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[NUM_AXIS] = { long target[NUM_AXIS] = {
lround(x * axis_steps_per_mm[X_AXIS]), lround(lx * axis_steps_per_mm[X_AXIS]),
lround(y * axis_steps_per_mm[Y_AXIS]), lround(ly * axis_steps_per_mm[Y_AXIS]),
lround(z * axis_steps_per_mm[Z_AXIS]), lround(lz * axis_steps_per_mm[Z_AXIS]),
lround(e * axis_steps_per_mm[E_AXIS]) lround(e * axis_steps_per_mm[E_AXIS])
}; };
@ -598,11 +604,22 @@ void Planner::buffer_line(
/* /*
SERIAL_ECHO_START; 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(" (", dx);
SERIAL_ECHOPAIR(") Y:", y); SERIAL_ECHOPAIR(") Y:", y);
#endif
SERIAL_ECHOPAIR(" (", dy); SERIAL_ECHOPAIR(" (", dy);
#elif ENABLED(DELTA)
SERIAL_ECHOPAIR(") C:", z);
#else
SERIAL_ECHOPAIR(") Z:", z); SERIAL_ECHOPAIR(") Z:", z);
#endif
SERIAL_ECHOPAIR(" (", dz); SERIAL_ECHOPAIR(" (", dz);
SERIAL_ECHOLNPGM(")"); SERIAL_ECHOLNPGM(")");
//*/ //*/
@ -671,7 +688,7 @@ void Planner::buffer_line(
// For a mixing extruder, get a magnified step_event_count for each // For a mixing extruder, get a magnified step_event_count for each
#if ENABLED(MIXING_EXTRUDER) #if ENABLED(MIXING_EXTRUDER)
for (uint8_t i = 0; i < MIXING_STEPPERS; i++) 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 #endif
#if FAN_COUNT > 0 #if FAN_COUNT > 0
@ -1152,22 +1169,15 @@ void Planner::buffer_line(
* *
* On CORE machines stepper ABC will be translated from the given XYZ. * On CORE machines stepper ABC will be translated from the given XYZ.
*/ */
void Planner::set_position_mm( void Planner::set_position_mm(ARG_X, ARG_Y, ARG_Z, const float &e) {
#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 ENABLED(MESH_BED_LEVELING) || ENABLED(AUTO_BED_LEVELING_FEATURE) #if PLANNER_LEVELING
apply_leveling(x, y, z); apply_leveling(lx, ly, lz);
#endif #endif
long nx = position[X_AXIS] = lround(x * axis_steps_per_mm[X_AXIS]), long nx = position[X_AXIS] = lround(lx * axis_steps_per_mm[X_AXIS]),
ny = position[Y_AXIS] = lround(y * axis_steps_per_mm[Y_AXIS]), ny = position[Y_AXIS] = lround(ly * axis_steps_per_mm[Y_AXIS]),
nz = position[Z_AXIS] = lround(z * axis_steps_per_mm[Z_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]); ne = position[E_AXIS] = lround(e * axis_steps_per_mm[E_AXIS]);
stepper.set_position(nx, ny, nz, ne); stepper.set_position(nx, ny, nz, ne);
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.

32
Marlin/planner.h
View File

@ -202,11 +202,24 @@ class Planner {
static bool is_full() { return (block_buffer_tail == BLOCK_MOD(block_buffer_head + 1)); } 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(AUTO_BED_LEVELING_FEATURE) || ENABLED(MESH_BED_LEVELING)
#define ARG_X float lx
#if ENABLED(MESH_BED_LEVELING) #define ARG_Y float ly
static void apply_leveling(const float &x, const float &y, float &z); #define ARG_Z float lz
#else #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 #endif
/** /**
@ -216,7 +229,7 @@ class Planner {
* fr_mm_s - (target) speed of the move (mm/s) * fr_mm_s - (target) speed of the move (mm/s)
* extruder - target extruder * 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. * Set the planner.position and individual stepper positions.
@ -227,14 +240,7 @@ class Planner {
* *
* Clears previous speed values. * Clears previous speed values.
*/ */
static void set_position_mm(float x, float y, float z, const float& e); static void set_position_mm(ARG_X, ARG_Y, ARG_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
/** /**
* Set the E position (mm) of the planner (and the E stepper) * Set the E position (mm) of the planner (and the E stepper)