🚸 Universal X_AXIS_TWIST_COMPENSATION (#23828)

2 years ago · 575c3150f9
10 changed files with 67 additions and 31 deletions
--- a/Marlin/Configuration_adv.h
+++ b/Marlin/Configuration_adv.h
@ -1299,7 +1299,7 @@

 #if HAS_MARLINUI_MENU

-  #if BOTH(HAS_BED_PROBE, AUTO_BED_LEVELING_BILINEAR)
+  #if HAS_BED_PROBE
    // Add calibration in the Probe Offsets menu to compensate for X-axis twist.
    //#define X_AXIS_TWIST_COMPENSATION
    #if ENABLED(X_AXIS_TWIST_COMPENSATION)
@ -1311,6 +1311,7 @@
      #define XATC_START_Z 0.0
      #define XATC_MAX_POINTS 3             // Number of points to probe in the wizard
      #define XATC_Y_POSITION Y_CENTER      // (mm) Y position to probe
+      #define XATC_Z_OFFSETS { 0, 0, 0 }    // Z offsets for X axis sample points
    #endif
  #endif

--- a/Marlin/src/feature/bedlevel/bedlevel.h
+++ b/Marlin/src/feature/bedlevel/bedlevel.h
@ -63,9 +63,6 @@ class TemporaryBedLevelingState {

  #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
    #include "abl/abl.h"
-    #if ENABLED(X_AXIS_TWIST_COMPENSATION)
-      #include "abl/x_twist.h"
-    #endif
  #elif ENABLED(AUTO_BED_LEVELING_UBL)
    #include "ubl/ubl.h"
  #elif ENABLED(MESH_BED_LEVELING)
--- a/Marlin/src/feature/bedlevel/abl/x_twist.cpp
+++ b/Marlin/src/feature/bedlevel/abl/x_twist.cpp
@ -19,16 +19,24 @@
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 */
-#include "../../../inc/MarlinConfig.h"
+#include "../inc/MarlinConfig.h"

 #if ENABLED(X_AXIS_TWIST_COMPENSATION)

-#include "../bedlevel.h"
+#include "x_twist.h"

 XATC xatc;

+bool XATC::enabled = true;
 float XATC::spacing, XATC::start;
-xatc_array_t XATC::z_offset;
+xatc_array_t XATC::z_offset; // Initialized by settings.load()
+
+void XATC::reset() {
+  constexpr float xzo[] = XATC_Z_OFFSETS;
+  static_assert(COUNT(xzo) == XATC_MAX_POINTS, "XATC_Z_OFFSETS is the wrong size.");
+  enabled = false;
+  COPY(z_offset, xzo);
+}

 void XATC::print_points() {
  SERIAL_ECHOLNPGM(" X-Twist Correction:");
@ -49,6 +57,7 @@ void XATC::print_points() {
 float lerp(const_float_t t, const_float_t a, const_float_t b) { return a + t * (b - a); }

 float XATC::compensation(const xy_pos_t &raw) {
+  if (!enabled) return 0;
  if (NEAR_ZERO(spacing)) return 0;
  float t = (raw.x - start) / spacing;
  int i = FLOOR(t);
--- a/Marlin/src/feature/bedlevel/abl/x_twist.h
+++ b/Marlin/src/feature/bedlevel/abl/x_twist.h
@ -21,15 +21,18 @@
 */
 #pragma once

-#include "../../../inc/MarlinConfigPre.h"
+#include "../inc/MarlinConfigPre.h"

 typedef float xatc_array_t[XATC_MAX_POINTS];

 class XATC {
+  static bool enabled;
 public:
  static float spacing, start;
  static xatc_array_t z_offset;

+  static void reset();
+  static void set_enabled(const bool ena) { enabled = ena; }
  static float compensation(const xy_pos_t &raw);
  static void print_points();
 };
--- a/Marlin/src/gcode/bedlevel/abl/G29.cpp
+++ b/Marlin/src/gcode/bedlevel/abl/G29.cpp
@ -681,7 +681,7 @@ G29_TYPE GcodeSuite::G29() {
          #elif ENABLED(AUTO_BED_LEVELING_BILINEAR)

            const float z = abl.measured_z + abl.Z_offset;
-            z_values[abl.meshCount.x][abl.meshCount.y] = z PLUS_TERN0(X_AXIS_TWIST_COMPENSATION, xatc.compensation(abl.probePos));
+            z_values[abl.meshCount.x][abl.meshCount.y] = z;
            TERN_(EXTENSIBLE_UI, ExtUI::onMeshUpdate(abl.meshCount, z));

          #endif
--- a/Marlin/src/lcd/menu/menu_x_twist.cpp
+++ b/Marlin/src/lcd/menu/menu_x_twist.cpp
@ -27,6 +27,7 @@
 #include "menu_addon.h"
 #include "../../module/planner.h"
 #include "../../feature/bedlevel/bedlevel.h"
+#include "../../feature/x_twist.h"
 #include "../../module/motion.h"
 #include "../../gcode/queue.h"
 #include "../../module/probe.h"
@ -148,7 +149,9 @@ void xatc_wizard_goto_next_point() {
      // Deploy certain probes before starting probing
      TERN_(BLTOUCH, do_z_clearance(Z_CLEARANCE_DEPLOY_PROBE));

+      xatc.set_enabled(false);
      measured_z = probe.probe_at_point(x, XATC_Y_POSITION, PROBE_PT_STOW);
+      xatc.set_enabled(true);
      current_position += probe.offset_xy;
      current_position.z = XATC_START_Z - probe.offset.z + measured_z;
      line_to_current_position(MMM_TO_MMS(XY_PROBE_FEEDRATE));
--- a/Marlin/src/module/probe.cpp
+++ b/Marlin/src/module/probe.cpp
@ -81,6 +81,10 @@
  #include "../feature/probe_temp_comp.h"
 #endif

+#if ENABLED(X_AXIS_TWIST_COMPENSATION)
+  #include "../feature/x_twist.h"
+#endif
+
 #if ENABLED(EXTENSIBLE_UI)
  #include "../lcd/extui/ui_api.h"
 #elif ENABLED(DWIN_CREALITY_LCD_ENHANCED)
@ -808,6 +812,7 @@ float Probe::probe_at_point(const_float_t rx, const_float_t ry, const ProbePtRai
  if (!deploy()) {
    measured_z = run_z_probe(sanity_check) + offset.z;
    TERN_(HAS_PTC, ptc.apply_compensation(measured_z));
+    TERN_(X_AXIS_TWIST_COMPENSATION, measured_z += xatc.compensation(npos + offset_xy));
  }
  if (!isnan(measured_z)) {
    const bool big_raise = raise_after == PROBE_PT_BIG_RAISE;
--- a/Marlin/src/module/settings.cpp
+++ b/Marlin/src/module/settings.cpp
@ -64,7 +64,7 @@
 #if HAS_LEVELING
  #include "../feature/bedlevel/bedlevel.h"
  #if ENABLED(X_AXIS_TWIST_COMPENSATION)
-    #include "../feature/bedlevel/abl/x_twist.h"
+    #include "../feature/x_twist.h"
  #endif
 #endif

@ -269,13 +269,17 @@ typedef struct SettingsDataStruct {
  xy_pos_t bilinear_grid_spacing, bilinear_start;       // G29 L F
  #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
    bed_mesh_t z_values;                                // G29
-    #if ENABLED(X_AXIS_TWIST_COMPENSATION)
-      XATC xatc;                                        // TBD
-    #endif
  #else
    float z_values[3][3];
  #endif

+  //
+  // X_AXIS_TWIST_COMPENSATION
+  //
+  #if ENABLED(X_AXIS_TWIST_COMPENSATION)
+    XATC xatc;                                          // TBD
+  #endif
+
  //
  // AUTO_BED_LEVELING_UBL
  //
@ -298,7 +302,7 @@ typedef struct SettingsDataStruct {
      int16_t z_offsets_bed[COUNT(ptc.z_offsets_bed)];     // M871 B I V
    #endif
    #if ENABLED(PTC_HOTEND)
-      int16_t z_offsets_hotend[COUNT(ptc.z_offsets_hotend)];     // M871 E I V
+      int16_t z_offsets_hotend[COUNT(ptc.z_offsets_hotend)]; // M871 E I V
    #endif
  #endif

@ -873,9 +877,6 @@ void MarlinSettings::postprocess() {
          sizeof(z_values) == (GRID_MAX_POINTS) * sizeof(z_values[0][0]),
          "Bilinear Z array is the wrong size."
        );
-        #if ENABLED(X_AXIS_TWIST_COMPENSATION)
-          static_assert(COUNT(xatc.z_offset) == XATC_MAX_POINTS, "XATC Z-offset mesh is the wrong size.");
-        #endif
      #else
        const xy_pos_t bilinear_start{0}, bilinear_grid_spacing{0};
      #endif
@ -889,15 +890,20 @@ void MarlinSettings::postprocess() {

      #if ENABLED(AUTO_BED_LEVELING_BILINEAR)
        EEPROM_WRITE(z_values);              // 9-256 floats
-        #if ENABLED(X_AXIS_TWIST_COMPENSATION)
-          EEPROM_WRITE(xatc);
-        #endif
      #else
        dummyf = 0;
        for (uint16_t q = grid_max_x * grid_max_y; q--;) EEPROM_WRITE(dummyf);
      #endif
    }

+    //
+    // X Axis Twist Compensation
+    //
+    #if ENABLED(X_AXIS_TWIST_COMPENSATION)
+      _FIELD_TEST(xatc);
+      EEPROM_WRITE(xatc);
+    #endif
+
    //
    // Unified Bed Leveling
    //
@ -1785,9 +1791,6 @@ void MarlinSettings::postprocess() {
            EEPROM_READ(bilinear_grid_spacing);        // 2 ints
            EEPROM_READ(bilinear_start);               // 2 ints
            EEPROM_READ(z_values);                     // 9 to 256 floats
-            #if ENABLED(X_AXIS_TWIST_COMPENSATION)
-              EEPROM_READ(xatc);
-            #endif
          }
          else // EEPROM data is stale
        #endif // AUTO_BED_LEVELING_BILINEAR
@ -1800,6 +1803,13 @@ void MarlinSettings::postprocess() {
          }
      }

+      //
+      // X Axis Twist Compensation
+      //
+      #if ENABLED(X_AXIS_TWIST_COMPENSATION)
+        EEPROM_READ(xatc);
+      #endif
+
      //
      // Unified Bed Leveling active state
      //
@ -2849,6 +2859,14 @@ void MarlinSettings::reset() {
  TERN_(ENABLE_LEVELING_FADE_HEIGHT, new_z_fade_height = (DEFAULT_LEVELING_FADE_HEIGHT));
  TERN_(HAS_LEVELING, reset_bed_level());

+  //
+  // X Axis Twist Compensation
+  //
+  TERN_(X_AXIS_TWIST_COMPENSATION, xatc.reset());
+
+  //
+  // Nozzle-to-probe Offset
+  //
  #if HAS_BED_PROBE
    constexpr float dpo[] = NOZZLE_TO_PROBE_OFFSET;
    static_assert(COUNT(dpo) == LINEAR_AXES, "NOZZLE_TO_PROBE_OFFSET must contain offsets for each linear axis X, Y, Z....");
@ -3313,14 +3331,14 @@ void MarlinSettings::reset() {
          }
        }

-        // TODO: Create G-code for settings
-        //#if ENABLED(X_AXIS_TWIST_COMPENSATION)
-        //  CONFIG_ECHO_START();
-        //  xatc.print_points();
-        //#endif
-
      #endif

+      // TODO: Create G-code for settings
+      //#if ENABLED(X_AXIS_TWIST_COMPENSATION)
+      //  CONFIG_ECHO_START();
+      //  xatc.print_points();
+      //#endif
+
    #endif // HAS_LEVELING

    //
--- a/ini/features.ini
+++ b/ini/features.ini
@ -98,7 +98,7 @@ USB_FLASH_DRIVE_SUPPORT                = src_filter=+<src/sd/usb_flashdrive/Sd2C
 HAS_MCP3426_ADC                        = src_filter=+<src/feature/adc> +<src/gcode/feature/adc>
 AUTO_BED_LEVELING_BILINEAR             = src_filter=+<src/feature/bedlevel/abl>
 AUTO_BED_LEVELING_(3POINT|(BI)?LINEAR) = src_filter=+<src/gcode/bedlevel/abl>
-X_AXIS_TWIST_COMPENSATION              = src_filter=+<src/feature/bedlevel/abl/x_twist.cpp> +<src/lcd/menu/menu_x_twist.cpp>
+X_AXIS_TWIST_COMPENSATION              = src_filter=+<src/feature/x_twist.cpp> +<src/lcd/menu/menu_x_twist.cpp>
 MESH_BED_LEVELING                      = src_filter=+<src/feature/bedlevel/mbl> +<src/gcode/bedlevel/mbl>
 AUTO_BED_LEVELING_UBL                  = src_filter=+<src/feature/bedlevel/ubl> +<src/gcode/bedlevel/ubl>
 UBL_HILBERT_CURVE                      = src_filter=+<src/feature/bedlevel/hilbert_curve.cpp>
--- a/platformio.ini
+++ b/platformio.ini
@ -100,7 +100,6 @@ default_src_filter = +<src/*> -<src/config> -<src/HAL> +<src/HAL/shared>
  -<src/feature/backlash.cpp>
  -<src/feature/baricuda.cpp> -<src/gcode/feature/baricuda>
  -<src/feature/bedlevel/abl> -<src/gcode/bedlevel/abl>
-  -<src/feature/bedlevel/abl/x_twist.cpp>
  -<src/feature/bedlevel/mbl> -<src/gcode/bedlevel/mbl>
  -<src/feature/bedlevel/ubl> -<src/gcode/bedlevel/ubl>
  -<src/feature/bedlevel/hilbert_curve.cpp>
@ -151,6 +150,7 @@ default_src_filter = +<src/*> -<src/config> -<src/HAL> +<src/HAL/shared>
  -<src/feature/tmc_util.cpp> -<src/module/stepper/trinamic.cpp>
  -<src/feature/tramming.cpp>
  -<src/feature/twibus.cpp>
+  -<src/feature/x_twist.cpp>
  -<src/feature/z_stepper_align.cpp>
  -<src/gcode/bedlevel/G26.cpp>
  -<src/gcode/bedlevel/G35.cpp>