From c109399bf6363741906a556092a72307638a7ce6 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Sun, 11 Sep 2016 21:40:44 -0500 Subject: [PATCH] Fix planner leveling and rename arguments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Use lx, ly, lz for “logical” positions --- Marlin/planner.cpp | 110 ++++++++++++++++++++++++--------------------- Marlin/planner.h | 66 ++++++++++++++------------- 2 files changed, 96 insertions(+), 80 deletions(-) diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index f8dc5efd2b..93084792f1 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -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. diff --git a/Marlin/planner.h b/Marlin/planner.h index ecca0fd3fc..e38e9e5f03 100644 --- a/Marlin/planner.h +++ b/Marlin/planner.h @@ -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)