axis_twist_compensation: AXIS_TWIST_COMPENSATION new parameter AUTO f…

…or autocalibration This commit adds automatic calculation support for compensating X and Y axis twist in the axis_twist_compensation module. Signed-off-by: Jorge Apaza Merma <[email protected]>
Klipper3d · Oct 23, 2024 · f3653e7 · f3653e7
1 parent 9f2de44
commit f3653e7
Show file tree

Hide file tree

Showing 4 changed files with 222 additions and 21 deletions.
diff --git a/docs/Axis_Twist_Compensation.md b/docs/Axis_Twist_Compensation.md
@@ -24,20 +24,51 @@ try to probe the bed without attaching the probe if you use it.
 > **Tip:** Make sure the [probe X and Y offsets](Config_Reference.md#probe) are
 > correctly set as they greatly influence calibration.
 
-1. After setting up the [axis_twist_compensation] module,
-perform `AXIS_TWIST_COMPENSATION_CALIBRATE`
-* The calibration wizard will prompt you to measure the probe Z offset at a few
-points along the bed
-* The calibration defaults to 3 points but you can use the option
-`SAMPLE_COUNT=` to use a different number.
-* For Y-axis calibration, use `AXIS_TWIST_COMPENSATION_CALIBRATE AXIS=Y` instead.
-2. [Adjust your Z offset](Probe_Calibrate.md#calibrating-probe-z-offset)
-3. Perform automatic/probe-based bed tramming operations, such as
-[Screws Tilt Adjust](G-Codes.md#screws_tilt_adjust),
-[Z Tilt Adjust](G-Codes.md#z_tilt_adjust) etc
-4. Home all axis, then perform a [Bed Mesh](Bed_Mesh.md) if required
-5. Perform a test print, followed by any
-[fine-tuning](Axis_Twist_Compensation.md#fine-tuning) as desired
+### Basic Usage: X-Axis Calibration
+1. After setting up the `[axis_twist_compensation]` module, run:
+``
+AXIS_TWIST_COMPENSATION_CALIBRATE
+``
+This command will calibrate the X-axis by default.
+    - The calibration wizard will prompt you to measure the probe Z offset at 
+    several points along the bed.
+    - By default, the calibration uses 3 points, but you can specify a different
+    number with the option:
+``
+SAMPLE_COUNT=<value>
+``
+
+2. **Adjust Your Z Offset:**
+After completing the calibration, be sure to [adjust your Z offset]
+(Probe_Calibrate.md#calibrating-probe-z-offset).
+
+3. **Perform Bed Leveling Operations:**
+Use probe-based operations as needed, such as:
+    - [Screws Tilt Adjust](G-Codes.md#screws_tilt_adjust)
+    - [Z Tilt Adjust](G-Codes.md#z_tilt_adjust)
+
+4. **Finalize the Setup:**
+    - Home all axes, and perform a [Bed Mesh](Bed_Mesh.md) if necessary.
+    - Run a test print, followed by any
+    [fine-tuning](Axis_Twist_Compensation.md#fine-tuning)
+    if needed.
+
+### For Y-Axis Calibration
+The calibration process for the Y-axis is similar to the X-axis. To calibrate
+the Y-axis, use:
+``
+AXIS_TWIST_COMPENSATION_CALIBRATE AXIS=Y
+``
+This will guide you through the same measuring process as for the X-axis.
+
+### Automatic Calibration for Both Axes
+To perform automatic calibration for both the X and Y axes without manual
+intervention, use:
+``
+AXIS_TWIST_COMPENSATION_CALIBRATE AUTO=True
+``
+In this mode, the calibration process will run for both axes automatically.
+
 
 > **Tip:** Bed temperature and nozzle temperature and size do not seem to have
 > an influence to the calibration process.

diff --git a/docs/Config_Reference.md b/docs/Config_Reference.md
@@ -2035,9 +2035,9 @@ sensor_type: ldc1612
 
 ### [axis_twist_compensation]
 
-A tool to compensate for inaccurate probe readings due to twist in X or Y gantry. See
-the [Axis Twist Compensation Guide](Axis_Twist_Compensation.md) for more
-detailed information regarding symptoms, configuration and setup.
+A tool to compensate for inaccurate probe readings due to twist in X or Y
+gantry. See the [Axis Twist Compensation Guide](Axis_Twist_Compensation.md)
+for more detailed information regarding symptoms, configuration and setup.
 
 ```
 [axis_twist_compensation]

diff --git a/docs/G-Codes.md b/docs/G-Codes.md
@@ -146,9 +146,19 @@ The following commands are available when the
 section](Config_Reference.md#axis_twist_compensation) is enabled.
 
 #### AXIS_TWIST_COMPENSATION_CALIBRATE
-`AXIS_TWIST_COMPENSATION_CALIBRATE [SAMPLE_COUNT=<value>] [AXIS=<X or Y, default X>]`: Initiates the X or Y
-twist calibration wizard. `SAMPLE_COUNT` specifies the number of points along
-the X or Y axis to calibrate at and defaults to 3.
+`AXIS_TWIST_COMPENSATION_CALIBRATE [AXIS=<X|Y>] [AUTO=<True|False>]
+[SAMPLE_COUNT=<value>]`
+
+Calibrates axis twist compensation by specifying the target axis or
+enabling automatic calibration.
+
+- **AXIS:** Define the axis (`X` or `Y`) for which the twist compensation
+will be calibrated. If not specified, the axis defaults to `'X'`.
+
+- **AUTO:** Enables automatic calibration mode. When `AUTO=True`, the
+calibration will run for both the X and Y axes. In this mode, `AXIS`
+cannot be specified. If both `AXIS` and `AUTO` are provided, an error
+will be raised.
 
 ### [bed_mesh]
 

diff --git a/klippy/extras/axis_twist_compensation.py b/klippy/extras/axis_twist_compensation.py
@@ -151,7 +151,20 @@ def _register_gcode_handlers(self):
     def cmd_AXIS_TWIST_COMPENSATION_CALIBRATE(self, gcmd):
         self.gcmd = gcmd
         sample_count = gcmd.get_int('SAMPLE_COUNT', DEFAULT_SAMPLE_COUNT)
-        axis = gcmd.get('AXIS', 'X')
+        axis = gcmd.get('AXIS', None)
+        auto = gcmd.get('AUTO', False)
+
+        if axis is not None and auto:
+            raise self.gcmd.error(
+                "Cannot use both 'AXIS' and 'AUTO' at the same time."
+            )
+
+        if auto:
+            self._start_autocalibration(sample_count)
+            return
+
+        if axis is None and not auto:
+            axis = 'X'
 
         # check for valid sample_count
         if sample_count < 2:
@@ -231,6 +244,153 @@ def cmd_AXIS_TWIST_COMPENSATION_CALIBRATE(self, gcmd):
         self.current_axis = axis
         self._calibration(probe_points, nozzle_points, interval_dist)
 
+    def _calculate_corrections(self, coordinates):
+        # Extracting x, y, and z values from coordinates
+        x_coords = [coord[0] for coord in coordinates]
+        y_coords = [coord[1] for coord in coordinates]
+        z_coords = [coord[2] for coord in coordinates]
+
+        # Calculate the desired point (average of all corner points in z)
+        # For a general case, we should extract the unique
+        # combinations of corner points
+        z_corners = [z_coords[i] for i, coord in enumerate(coordinates)
+                        if (coord[0] in [x_coords[0], x_coords[-1]])
+                        and (coord[1] in [y_coords[0], y_coords[-1]])]
+        z_desired = sum(z_corners) / len(z_corners)
+
+
+        # Calculate average deformation per axis
+        unique_x_coords = sorted(set(x_coords))
+        unique_y_coords = sorted(set(y_coords))
+
+        avg_z_x = []
+        for x in unique_x_coords:
+            indices = [i for i, coord in enumerate(coordinates)
+                        if coord[0] == x]
+            avg_z = sum(z_coords[i] for i in indices) / len(indices)
+            avg_z_x.append(avg_z)
+
+        avg_z_y = []
+        for y in unique_y_coords:
+            indices = [i for i, coord in enumerate(coordinates)
+                        if coord[1] == y]
+            avg_z = sum(z_coords[i] for i in indices) / len(indices)
+            avg_z_y.append(avg_z)
+
+        # Calculate corrections to reach the desired point
+        x_corrections = [z_desired - avg for avg in avg_z_x]
+        y_corrections = [z_desired - avg for avg in avg_z_y]
+
+        return x_corrections, y_corrections
+
+    def _start_autocalibration(self, sample_count):
+
+        if not all([
+                self.x_start_point[0],
+                self.x_end_point[0],
+                self.y_start_point[0],
+                self.y_end_point[0]
+                ]):
+                raise self.gcmd.error(
+                    """AXIS_TWIST_COMPENSATION_AUTOCALIBRATE requires
+                    calibrate_start_x, calibrate_end_x, calibrate_start_y
+                    and calibrate_end_y to be defined
+                    """
+                    )
+
+        # check for valid sample_count
+        if sample_count is None or sample_count < 2:
+            raise self.gcmd.error(
+                "SAMPLE_COUNT to probe must be at least 2")
+
+        # verify no other manual probe is in progress
+        manual_probe.verify_no_manual_probe(self.printer)
+
+        # clear the current config
+        self.compensation.clear_compensations()
+
+        min_x = self.x_start_point[0]
+        max_x = self.x_end_point[0]
+        min_y = self.y_start_point[1]
+        max_y = self.y_end_point[1]
+
+        # calculate x positions
+        interval_x = (max_x - min_x) / (sample_count - 1)
+        xps = [min_x + interval_x * i for i in range(sample_count)]
+
+        # Calculate points array
+        interval_y = (max_y - min_y) / (sample_count - 1)
+        flip = False
+
+        points = []
+        for i in range(sample_count):
+            for j in range(sample_count):
+                if(not flip):
+                    idx = j
+                else:
+                    idx = sample_count -1 - j
+                points.append([xps[i], min_y + interval_y * idx ])
+            flip = not flip
+
+
+        # calculate the points to put the nozzle at, and probe
+        probe_points = []
+
+        for i in range(len(points)):
+            x = points[i][0] - self.probe_x_offset
+            y = points[i][1] - self.probe_y_offset
+            probe_points.append([x, y, self._auto_calibration((x,y))[2]])
+
+        # calculate corrections
+        x_corr, y_corr = self._calculate_corrections(probe_points)
+
+        x_corr_str = ', '.join(["{:.6f}".format(x)
+                                    for x in x_corr])
+
+        y_corr_str = ', '.join(["{:.6f}".format(x)
+                                    for x in y_corr])
+
+        # finalize
+        configfile = self.printer.lookup_object('configfile')
+        configfile.set(self.configname, 'z_compensations', x_corr_str)
+        configfile.set(self.configname, 'compensation_start_x',
+                    self.x_start_point[0])
+        configfile.set(self.configname, 'compensation_end_x',
+                    self.x_end_point[0])
+
+
+        configfile.set(self.configname, 'zy_compensations', y_corr_str)
+        configfile.set(self.configname, 'compensation_start_y',
+                    self.y_start_point[1])
+        configfile.set(self.configname, 'compensation_end_y',
+                    self.y_end_point[1])
+
+        self.gcode.respond_info(
+            "AXIS_TWIST_COMPENSATION state has been saved "
+            "for the current session.  The SAVE_CONFIG command will "
+            "update the printer config file and restart the printer.")
+        # output result
+        self.gcmd.respond_info(
+            "AXIS_TWIST_COMPENSATION_AUTOCALIBRATE: Calibration complete: ")
+        self.gcmd.respond_info("\n".join(map(str, [x_corr, y_corr])), log=False)
+
+    def _auto_calibration(self, probe_point):
+
+        # horizontal_move_z (to prevent probe trigger or hitting bed)
+        self._move_helper((None, None, self.horizontal_move_z))
+
+        # move to point to probe
+        self._move_helper((probe_point[0],
+                            probe_point[1], None))
+
+        # probe the point
+        pos = probe.run_single_probe(self.probe, self.gcmd)
+
+        # horizontal_move_z (to prevent probe trigger or hitting bed)
+        self._move_helper((None, None, self.horizontal_move_z))
+
+        return pos
+
     def _calculate_probe_points(self, nozzle_points,
         probe_x_offset, probe_y_offset):
         # calculate the points to put the nozzle at