diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md index 8b17d9502..521c22d5f 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..f67427065 --- /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): + # 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,))