🚸 Universal X_AXIS_TWIST_COMPENSATION (MarlinFirmware#23828)

rwoelk · Mar 2, 2022 · 2e39bc3 · 2e39bc3
1 parent b07a34e
commit 2e39bc3
Show file tree

Hide file tree

Showing 10 changed files with 67 additions and 31 deletions.
diff --git 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
 

diff --git 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)

diff --git a/Marlin/src/feature/bedlevel/abl/x_twist.cpp → Marlin/src/feature/x_twist.cpp b/Marlin/src/feature/bedlevel/abl/x_twist.cpp → Marlin/src/feature/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);

diff --git a/Marlin/src/feature/bedlevel/abl/x_twist.h → Marlin/src/feature/x_twist.h b/Marlin/src/feature/bedlevel/abl/x_twist.h → Marlin/src/feature/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();
 };

diff --git 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

diff --git 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));

diff --git 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;

diff --git 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
 
     //

diff --git 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>

diff --git 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>