From f762f5e5011d7c9d9ded6262d41f4f9d28159635 Mon Sep 17 00:00:00 2001 From: Marcio Teixeira Date: Wed, 2 May 2018 07:45:05 -0600 Subject: [PATCH] Improved backlash compensation for Hibiscus (T2220) --- Marlin/Conditionals_LulzBot.h | 52 +++++++++++++++++++++-------------- Marlin/Marlin_main.cpp | 1 - Marlin/planner.cpp | 6 ++++ 3 files changed, 38 insertions(+), 21 deletions(-) diff --git a/Marlin/Conditionals_LulzBot.h b/Marlin/Conditionals_LulzBot.h index 3c16d600f..def2d052a 100644 --- a/Marlin/Conditionals_LulzBot.h +++ b/Marlin/Conditionals_LulzBot.h @@ -13,7 +13,7 @@ * got disabled. */ -#define LULZBOT_FW_VERSION ".42" // Change this with each update +#define LULZBOT_FW_VERSION ".47" // Change this with each update #if ( \ !defined(LULZBOT_Gladiola_Mini) && \ @@ -1622,6 +1622,7 @@ void gcode_G29_with_retry() { \ set_bed_leveling_enabled(false); \ for(uint8_t i = 0; i < LULZBOT_NUM_REWIPES; i++) { \ + LULZBOT_BACKLASH_MEASUREMENT_START \ LULZBOT_ENABLE_PROBE_PINS(true); \ gcode_G29(); \ LULZBOT_ENABLE_PROBE_PINS(false); \ @@ -1704,8 +1705,10 @@ /****************************** BACKLASH COMPENSATION **************************/ #if defined(LULZBOT_USE_Z_BACKLASH_COMPENSATION) + #define LULZBOT_BACKLASH_MEASUREMENT_RESOLUTION 0.005 #define LULZBOT_BACKLASH_MEASUREMENT_LIMIT 0.5 + #define LULZBOT_BACKLASH_SMOOTHING_DISTANCE 1500 #if ENABLED(LULZBOT_Z_MIN_PROBE_USES_Z_MIN_ENDSTOP_PIN) #if defined(LULZBOT_Z_MIN_ENDSTOP_INVERTING) @@ -1721,9 +1724,9 @@ #endif #endif - #define LULZBOT_BACKLASH_MEASUREMENT_DECL int32_t z_backlash_steps = 0; + #define LULZBOT_BACKLASH_MEASUREMENT_DECL int32_t z_backlash_steps; #define LULZBOT_BACKLASH_MEASUREMENT_EXTERN extern int32_t z_backlash_steps; - #define LULZBOT_BACKLASH_MEASUREMENT_START z_backlash_steps = LULZBOT_BACKLASH_MEASUREMENT_LIMIT * planner.axis_steps_per_mm[Z_AXIS]; + #define LULZBOT_BACKLASH_MEASUREMENT_START z_backlash_steps = 0; #define LULZBOT_BACKLASH_MEASUREMENT \ { \ @@ -1733,27 +1736,36 @@ do_blocking_move_to_z(current_position[Z_AXIS] + LULZBOT_BACKLASH_MEASUREMENT_RESOLUTION, MMM_TO_MMS(Z_PROBE_SPEED_SLOW)); \ } \ const float measured_backlash_mm = current_position[Z_AXIS] - start_height; \ - /* Take the minimum backlash from all four corners, as we can only compensate for the shared amount */ \ - z_backlash_steps = min(z_backlash_steps, measured_backlash_mm * planner.axis_steps_per_mm[Z_AXIS]); \ + /* Average the backlash from all four corners */ \ + z_backlash_steps += 0.25 * measured_backlash_mm * planner.axis_steps_per_mm[Z_AXIS]; \ } #define LULZBOT_BACKLASH_COMPENSATION \ { \ - static bool last_z_direction; \ - static bool is_correction = false; \ - if(!is_correction && planner.leveling_active) { \ - const bool new_z_direction = TEST(dm, Z_AXIS); \ - /* When Z changes direction, insert backlash correction */ \ - if((last_z_direction != new_z_direction) && (dc != 0)) { \ - last_z_direction = new_z_direction; \ - int32_t saved_position[NUM_AXIS], tweaked_position[XYZE]; \ - COPY( saved_position, position); \ - COPY(tweaked_position, position); \ - tweaked_position[Z_AXIS] += dc < 0 ? -z_backlash_steps : z_backlash_steps; \ - is_correction = true; /* Avoid infinite recursion */ \ - _buffer_steps(tweaked_position, fr_mm_s, extruder); \ - is_correction = false; \ - COPY(position, saved_position); \ + static bool last_z_direction; \ + static int32_t residual_z_error = 0; \ + if(planner.leveling_active) { \ + if(dc != 0) { \ + const bool new_z_direction = TEST(dm, Z_AXIS); \ + /* When Z changes direction, add backlash correction to residual_z_error, \ + * to be compensated over one or more subsequent segments */ \ + if(last_z_direction != new_z_direction) { \ + last_z_direction = new_z_direction; \ + if(dc < 0) { \ + residual_z_error -= z_backlash_steps; \ + } else { \ + residual_z_error += z_backlash_steps; \ + } \ + } \ + /* Take up a portion of the residual_z_error in this segment, but \ + * only when the current segment travels along Z in the same \ + * direction as the residual error */ \ + if((dc > 0 && residual_z_error > 0) || (dc < 0 && residual_z_error < 0)) { \ + const float segment_len = sqrt(da*da+db*db); \ + const int32_t z_adj = residual_z_error * min(1.0, segment_len / LULZBOT_BACKLASH_SMOOTHING_DISTANCE); \ + dc += z_adj; \ + residual_z_error -= z_adj; \ + } \ } \ } \ } diff --git a/Marlin/Marlin_main.cpp b/Marlin/Marlin_main.cpp index 2849a3b16..cfbcc4cd8 100644 --- a/Marlin/Marlin_main.cpp +++ b/Marlin/Marlin_main.cpp @@ -11827,7 +11827,6 @@ void process_parsed_command( #if HAS_LEVELING case 29: // G29 Detailed Z probe, probes the bed at 3 or more points, // or provides access to the UBL System if enabled. - LULZBOT_BACKLASH_MEASUREMENT_START #if defined(LULZBOT_G29_COMMAND) LULZBOT_G29_COMMAND #else diff --git a/Marlin/planner.cpp b/Marlin/planner.cpp index 18d3c6571..0fefccef0 100644 --- a/Marlin/planner.cpp +++ b/Marlin/planner.cpp @@ -720,9 +720,15 @@ void Planner::check_axes_activity() { */ void Planner::_buffer_steps(const int32_t (&target)[XYZE], float fr_mm_s, const uint8_t extruder) { + #if defined(LULZBOT_USE_Z_BACKLASH_COMPENSATION) + int32_t da = target[X_AXIS] - position[X_AXIS], + db = target[Y_AXIS] - position[Y_AXIS], + dc = target[Z_AXIS] - position[Z_AXIS]; + #else const int32_t da = target[X_AXIS] - position[X_AXIS], db = target[Y_AXIS] - position[Y_AXIS], dc = target[Z_AXIS] - position[Z_AXIS]; + #endif int32_t de = target[E_AXIS] - position[E_AXIS];