From df5046c47627f76d0b16405b31a86dcfe9ddd709 Mon Sep 17 00:00:00 2001 From: Oleksii Shchetinin Date: Sun, 7 Sep 2025 00:17:05 +0300 Subject: [PATCH] backlash: Add backlash compensation module Add support for compensating mechanical backlash (lost motion) when axes reverse direction. This is common in lead screw and belt drive systems. The module supports two kinematics modes: - Cartesian: Independent X, Y, Z axis compensation - CoreXY: Compensation for A/B diagonal motors (A=X+Y, B=X-Y) Compensation is applied in real-time via move transform and is hidden from the UI position display. Direction tracking is reset on homing. Configuration example: [backlash] kinematics: corexy a: 0.05 b: 0.03 z: 0.1 Signed-off-by: Oleksii Shchetinin --- docs/Config_Reference.md | 42 +++++++ klippy/extras/backlash.py | 237 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 279 insertions(+) create mode 100644 klippy/extras/backlash.py diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index b01360adf..0fd15fa96 100644 --- a/docs/Config_Reference.md +++ b/docs/Config_Reference.md @@ -1442,6 +1442,48 @@ See also: [extended g-code commands](G-Codes.md#z_thermal_adjust). # parameter. ``` +### [backlash] + +Backlash compensation for mechanical play in axis drive systems. This module +compensates for the small amount of lost motion that occurs when an axis +reverses direction, which is common in lead screw and belt drive systems. +The compensation is applied in real-time during moves. + +Two kinematics modes are supported: +- **Cartesian**: Independent X, Y, Z axis compensation +- **CoreXY**: Compensation for A and B diagonal motors (where A=X+Y, B=X-Y) + +``` +[backlash] +#kinematics: cartesian +# The kinematics type for backlash compensation. Must be either +# "cartesian" or "corexy". Default is "cartesian". + +# For Cartesian kinematics: +#x: 0.0 +# Backlash compensation for the X axis in mm. Applied when the X axis +# changes direction from negative to positive. Default is 0.0 (disabled). +#y: 0.0 +# Backlash compensation for the Y axis in mm. Applied when the Y axis +# changes direction from negative to positive. Default is 0.0 (disabled). + +# For CoreXY kinematics: +#a: 0.0 +# Backlash compensation for the A motor in mm. The A motor controls the +# X+Y diagonal motion. Applied when the A motor changes direction from +# negative to positive. Default is 0.0 (disabled). +#b: 0.0 +# Backlash compensation for the B motor in mm. The B motor controls the +# X-Y diagonal motion. Applied when the B motor changes direction from +# negative to positive. Default is 0.0 (disabled). + +# Common to all kinematics: +#z: 0.0 +# Backlash compensation for the Z axis in mm. Applied when the Z axis +# changes direction from negative to positive (moving up). Default is +# 0.0 (disabled). +``` + ## Customized homing ### [safe_z_home] diff --git a/klippy/extras/backlash.py b/klippy/extras/backlash.py new file mode 100644 index 000000000..44ffa9a13 --- /dev/null +++ b/klippy/extras/backlash.py @@ -0,0 +1,237 @@ +# Backlash compensation. +# +# Copyright (C) 2025 Oleksii Shchetinin +# +# This file may be distributed under the terms of the GNU GPLv3 license. + + +class BacklashCompensation: + """Base class for backlash compensation.""" + + def __init__(self, config): + # System objects + self.printer = config.get_printer() + self.toolhead = None + self.root_transform = None + self.gcode_move = None + self.motion_report = None + + # Reporting tool for UI (hide compensation) + self.toolhead_status = None + self.motion_report_status = None + + # Z axis backlash (common for all kinematics) + self.backlash_z = config.getfloat('z', 0.0) + self.last_dir_z = 0 + + # Events handling registration + self.printer.register_event_handler("klippy:connect", self._on_connect) + self.printer.register_event_handler("homing:home_rails_end", + self._on_homing_move_end) + + def _on_connect(self): + # System objects assignment + self.toolhead = self.printer.lookup_object("toolhead") + self.gcode_move = self.printer.lookup_object('gcode_move') + self.motion_report = self.printer.lookup_object("motion_report") + + # Updates UI display section + self.toolhead_status = self.toolhead.get_status + self.toolhead.get_status = self._toolhead_status_interceptor + self.motion_report_status = self.motion_report.get_status + self.motion_report.get_status = self._motion_report_status_interceptor + + # Movement transform registration (calls original) + self.root_transform = self.gcode_move.set_move_transform( + self, force=True) + + def _on_homing_move_end(self, homing_state, rails): + axes = homing_state.get_axes() + self._reset_xy_directions(axes) + if 2 in axes: + self.last_dir_z = -1 + + def _reset_xy_directions(self, axes): + """Reset X/Y direction tracking after homing. Override in subclass.""" + pass + + def _get_xy_offset(self): + """Get current X/Y offset. Override in subclass.""" + return 0.0, 0.0 + + def _update_xy_directions(self, start_x, start_y, end_x, end_y): + """Update X/Y direction tracking. Override in subclass.""" + pass + + def _get_current_offset(self): + """Calculate current XYZ offset based on direction state.""" + offset_x, offset_y = self._get_xy_offset() + offset_z = self.backlash_z if self.last_dir_z == 1 else 0.0 + return offset_x, offset_y, offset_z + + def get_position(self): + # Return logical position (without backlash offset applied) + pos = list(self.root_transform.get_position()) + offset_x, offset_y, offset_z = self._get_current_offset() + + if offset_x != 0.0 or offset_y != 0.0 or offset_z != 0.0: + pos[0] -= offset_x + pos[1] -= offset_y + pos[2] -= offset_z + return pos + + def move(self, newpos, speed) -> None: + # Get current logical position + cur_pos = self.get_position() + start_x, start_y, start_z = cur_pos[0], cur_pos[1], cur_pos[2] + end_x, end_y, end_z = newpos[0], newpos[1], newpos[2] + dz = end_z - start_z + + # Update X/Y direction states (subclass specific) + self._update_xy_directions(start_x, start_y, end_x, end_y) + + # Update Z direction state + if dz != 0.0: + dir_now = 1 if dz > 0 else -1 + if self.last_dir_z != 0: + self.last_dir_z = dir_now + + # Calculate and apply offsets + offset_x, offset_y, offset_z = self._get_current_offset() + + adjusted_newpos = list(newpos) + adjusted_newpos[0] += offset_x + adjusted_newpos[1] += offset_y + adjusted_newpos[2] += offset_z + + self.root_transform.move(adjusted_newpos, speed) + + def _toolhead_status_interceptor(self, eventtime): + # Updates static position display (hide compensation from UI) + res = self.toolhead_status(eventtime) + offset_x, offset_y, offset_z = self._get_current_offset() + + if offset_x != 0.0 or offset_y != 0.0 or offset_z != 0.0: + commanded_pos = list(self.toolhead.commanded_pos[:4]) + commanded_pos[0] -= offset_x + commanded_pos[1] -= offset_y + commanded_pos[2] -= offset_z + res['position'] = self.toolhead.Coord(*commanded_pos) + return res + + def _motion_report_status_interceptor(self, eventtime): + # Updates real-time motion tracking (hide compensation from UI) + res = self.motion_report_status(eventtime) + offset_x, offset_y, offset_z = self._get_current_offset() + + if offset_x != 0.0 or offset_y != 0.0 or offset_z != 0.0: + mutated_coord = list(res['live_position']) + mutated_coord[0] -= offset_x + mutated_coord[1] -= offset_y + mutated_coord[2] -= offset_z + res['live_position'] = self.toolhead.Coord(*mutated_coord) + return res + + +class CartesianBacklash(BacklashCompensation): + """Backlash compensation for Cartesian kinematics (independent X, Y, Z).""" + + def __init__(self, config): + super().__init__(config) + # Backlash values per axis + self.backlash_x = config.getfloat('x', 0.0) + self.backlash_y = config.getfloat('y', 0.0) + + # Direction tracking: 0=unknown, 1=positive, -1=negative + self.last_dir_x = 0 + self.last_dir_y = 0 + + def _reset_xy_directions(self, axes): + if 0 in axes: + self.last_dir_x = -1 + if 1 in axes: + self.last_dir_y = -1 + + def _get_xy_offset(self): + offset_x = self.backlash_x if self.last_dir_x == 1 else 0.0 + offset_y = self.backlash_y if self.last_dir_y == 1 else 0.0 + return offset_x, offset_y + + def _update_xy_directions(self, start_x, start_y, end_x, end_y): + dx = end_x - start_x + dy = end_y - start_y + + if dx != 0.0: + dir_now = 1 if dx > 0 else -1 + if self.last_dir_x != 0: + self.last_dir_x = dir_now + + if dy != 0.0: + dir_now = 1 if dy > 0 else -1 + if self.last_dir_y != 0: + self.last_dir_y = dir_now + + +class CoreXYBacklash(BacklashCompensation): + """Backlash compensation for CoreXY kinematics (A/B diagonal motors).""" + + def __init__(self, config): + super().__init__(config) + # Backlash values per motor + # A = X + Y, B = X - Y + self.backlash_a = config.getfloat('a', 0.0) + self.backlash_b = config.getfloat('b', 0.0) + + # Direction tracking: 0=unknown, 1=positive, -1=negative + self.last_dir_a = 0 + self.last_dir_b = 0 + + def _xy_to_ab(self, x, y): + """Convert XY coordinates to AB motor positions.""" + a = x + y + b = x - y + return a, b + + def _reset_xy_directions(self, axes): + # X or Y homing affects both A and B motors in CoreXY + if 0 in axes or 1 in axes: + self.last_dir_a = -1 + self.last_dir_b = -1 + + def _get_xy_offset(self): + # Offset in AB space + offset_a = self.backlash_a if self.last_dir_a == 1 else 0.0 + offset_b = self.backlash_b if self.last_dir_b == 1 else 0.0 + # Convert to XY offset + offset_x = (offset_a + offset_b) / 2.0 + offset_y = (offset_a - offset_b) / 2.0 + return offset_x, offset_y + + def _update_xy_directions(self, start_x, start_y, end_x, end_y): + # Convert to AB space + start_a, start_b = self._xy_to_ab(start_x, start_y) + end_a, end_b = self._xy_to_ab(end_x, end_y) + + da = end_a - start_a + db = end_b - start_b + + if da != 0.0: + dir_now = 1 if da > 0 else -1 + if self.last_dir_a != 0: + self.last_dir_a = dir_now + + if db != 0.0: + dir_now = 1 if db > 0 else -1 + if self.last_dir_b != 0: + self.last_dir_b = dir_now + + +def load_config(config): + kinematics = config.get('kinematics', 'cartesian') + if kinematics == 'cartesian': + return CartesianBacklash(config) + elif kinematics == 'corexy': + return CoreXYBacklash(config) + else: + raise config.error("Unknown backlash kinematics '%s'. " + "Must be 'cartesian' or 'corexy'." % (kinematics,))