XY Skew correction #3839

Marlin/Conditionals_post.h

@@ -746,6 +746,14 @@
     #define Z_PROBE_OFFSET_FROM_EXTRUDER 0
   #endif
 
+  /**
+   * XY Skew
+   */
+  #if ENABLED(XY_SKEW_CORRECTION)
+    #define XY_SKEW_FACTOR_MIN -1
+    #define XY_SKEW_FACTOR_MAX 1
+  #endif
+
   /**
    * Heater & Fan Pausing
    */

Marlin/Configuration.h

@@ -1001,6 +1001,42 @@
 #define HOMING_FEEDRATE_XY (50*60)
 #define HOMING_FEEDRATE_Z  (4*60)
 
+//===========================================================================
+//=============================== Bed Skew ==============================
+//===========================================================================
+// @section bedskew
+/**
+ *	Using X axis as reference to fix the skew of bed the following steps must be done
+ * 		1. print a test square (example https://www.thingiverse.com/thing:2563185)
+		2. measure length of diagonal AC->D1
+ *		2. measure length of diagonal BD->D2
+ *		3. measure length AD->X1
+ *		4. compute side AB->Y1=sqrt(2*D1^2+2*D2^2-4*AD^2)/2
+ *		5. compute Xskew_angle=PI/2-ACOS(((D1^2-Y1^2-X1^2)/(2*Y1*X1)))
+ *		6. set XY_SKEW_XFACTOR=tan(Xskew_angle)
+ *
+ *    Y
+ *    ^
+ *    |          B----X2----C
+ *    |         / \      / /
+ *    |        /  D2  /   /
+ *    |       Y1    /    Y2
+ *    |      /   D1 \   /
+ *    |     / /      \ /
+ *    |    A----X1----D
+ *    |
+ *    +----------------------------> X   
+
+
+*/
+
+//uncomment to enable skew compensation
+//#define XY_SKEW_CORRECTION
+
+#if ENABLED(XY_SKEW_CORRECTION)
+  #define XY_SKEW_FACTOR 0.0
+#endif
+
 //=============================================================================
 //============================= Additional Features ===========================
 //=============================================================================

Marlin/Marlin_main.cpp

@@ -204,6 +204,7 @@
  * M666 - Set delta endstop adjustment. (Requires DELTA)
  * M605 - Set dual x-carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
  * M851 - Set Z probe's Z offset in current units. (Negative = below the nozzle.)
+ * M852 - Set XY skew correction factor.
  * M860 - Report the position of position encoder modules.
  * M861 - Report the status of position encoder modules.
  * M862 - Perform an axis continuity test for position encoder modules.
@@ -9676,9 +9677,31 @@ inline void gcode_M502() {
 
     SERIAL_EOL();
   }
-
+  
 #endif // HAS_BED_PROBE
 
+#if ENABLED(XY_SKEW_CORRECTION)
+
+  inline void gcode_M852() {
+    SERIAL_ECHO_START();
+    SERIAL_ECHOPGM(MSG_XY_SKEW_FACTOR " ");
+    if (parser.seen('F')) {
+      const float value = parser.value_linear_units();
+      if (WITHIN(value, XY_SKEW_FACTOR_MIN, XY_SKEW_FACTOR_MAX)) {
+        planner.xy_skew_factor = value;
+        SERIAL_ECHO(planner.xy_skew_factor);
+      }
+      else
+        SERIAL_ECHOPGM(MSG_XYSKEW_MIN " " STRINGIFY(XY_SKEW_FACTOR_MIN) " " MSG_XYSKEW_MAX " " STRINGIFY(XY_SKEW_FACTOR_MAX));
+    }
+    else
+      SERIAL_ECHOPAIR(": ", planner.xy_skew_factor);
+
+    SERIAL_EOL();
+  }
+
+#endif //XY_SKEW_CORRECTION
+
 #if ENABLED(ADVANCED_PAUSE_FEATURE)
 
   /**
@@ -11530,6 +11553,12 @@ void process_next_command() {
           gcode_M851();
           break;
       #endif // HAS_BED_PROBE
+
+	  #if ENABLED(XY_SKEW_CORRECTION)
+        case 852: // M852: Set XY Skew factor
+          gcode_M852();
+          break;
+      #endif
 
       #if ENABLED(ADVANCED_PAUSE_FEATURE)
         case 600: // M600: Pause for filament change

Marlin/configuration_store.cpp

@@ -36,7 +36,7 @@
  *
  */
 
-#define EEPROM_VERSION "V41"
+#define EEPROM_VERSION "V42"
 
 // Change EEPROM version if these are changed:
 #define EEPROM_OFFSET 100
@@ -162,8 +162,11 @@
  *  596  M907 Z    Stepper Z current                (uint32_t)
  *  600  M907 E    Stepper E current                (uint32_t)
  *
- *  604                                Minimum end-point
- * 1925 (604 + 36 + 9 + 288 + 988)     Maximum end-point
+ *  BED_SKEW_CORRECTION:                             8 bytes
+ *  604  M852      xy_skew_factor                   (float)
+ *
+ *  608                                Minimum end-point
+ * 1929 (608 + 36 + 9 + 288 + 988)     Maximum end-point
  *
  * ========================================================================
  * meshes_begin (between max and min end-point, directly above)
@@ -643,6 +646,13 @@ void MarlinSettings::postprocess() {
       const uint32_t dummyui32 = 0;
       for (uint8_t q = 3; q--;) EEPROM_WRITE(dummyui32);
     #endif
+
+	#if ENABLED(XY_SKEW_CORRECTION)
+      EEPROM_WRITE(planner.xy_skew_factor);
+    #else
+      const float dummyskew = 0.0f;
+      EEPROM_WRITE(dummyskew);
+    #endif
 
     if (!eeprom_error) {
       const int eeprom_size = eeprom_index;
@@ -1003,6 +1013,13 @@ void MarlinSettings::postprocess() {
         uint32_t dummyui32;
         for (uint8_t q = 3; q--;) EEPROM_READ(dummyui32);
       #endif
+
+	  #if ENABLED(XY_SKEW_CORRECTION)
+        EEPROM_READ(planner.xy_skew_factor);
+      #else
+        float dummyskew;
+        EEPROM_READ(dummyskew);
+      #endif
 
       if (working_crc == stored_crc) {
         postprocess();
@@ -1362,6 +1379,10 @@ void MarlinSettings::reset() {
     ubl.reset();
   #endif
 
+  #if ENABLED(XY_SKEW_CORRECTION)
+    planner.xy_skew_factor = XY_SKEW_FACTOR;
+  #endif
+
   postprocess();
 
   #if ENABLED(EEPROM_CHITCHAT)
@@ -1779,6 +1800,18 @@ void MarlinSettings::reset() {
       CONFIG_ECHO_START;
       SERIAL_ECHOLNPAIR("  M851 Z", LINEAR_UNIT(zprobe_zoffset));
     #endif
+
+    /**
+     * Bed Skew
+     */
+    #if ENABLED(XY_SKEW_CORRECTION)
+      if (!forReplay) {
+        CONFIG_ECHO_START;
+        SERIAL_ECHOLNPGM("XY Skew Factor:");
+      }
+      CONFIG_ECHO_START;
+      SERIAL_ECHOLNPAIR("  M852 F", LINEAR_UNIT(planner.xy_skew_factor));
+    #endif
 
     /**
      * TMC2130 stepper driver current

Marlin/language.h

@@ -153,6 +153,8 @@
 #define MSG_Y_MAX                           "y_max: "
 #define MSG_Z_MIN                           "z_min: "
 #define MSG_Z_MAX                           "z_max: "
+#define MSG_XYSKEW_MIN                      "min_xy_skew_factor: "
+#define MSG_XYSKEW_MAX                      "max_xy_skew_factor: "
 #define MSG_Z2_MIN                          "z2_min: "
 #define MSG_Z2_MAX                          "z2_max: "
 #define MSG_Z_PROBE                         "z_probe: "

Marlin/language_en.h

@@ -653,6 +653,9 @@
 #ifndef MSG_ZPROBE_ZOFFSET
   #define MSG_ZPROBE_ZOFFSET                  _UxGT("Z Offset")
 #endif
+#ifndef MSG_XY_SKEW_FACTOR
+  #define MSG_XY_SKEW_FACTOR                  _UxGT("XY Skew Factor")
+#endif
 #ifndef MSG_BABYSTEP_X
   #define MSG_BABYSTEP_X                      _UxGT("Babystep X")
 #endif

Marlin/planner.cpp

@@ -117,6 +117,10 @@ float Planner::min_feedrate_mm_s,
         Planner::inverse_z_fade_height;
 #endif
 
+#if ENABLED(XY_SKEW_CORRECTION)
+  float Planner::xy_skew_factor; // Initialized by settings.load()
+#endif
+
 #if ENABLED(AUTOTEMP)
   float Planner::autotemp_max = 250,
         Planner::autotemp_min = 210,
@@ -528,6 +532,22 @@ void Planner::check_axes_activity() {
    */
   void Planner::apply_leveling(float &lx, float &ly, float &lz) {
 
+	#if ENABLED(XY_SKEW_CORRECTION)
+		/*SERIAL_ECHOPAIR("X(logic): ", lx);
+		SERIAL_ECHOPAIR(" X(real): ", RAW_X_POSITION(lx));
+
+		SERIAL_ECHOPAIR(" ===> Y(logic): ", ly);
+		SERIAL_ECHOLNPAIR(" Y(real): ", RAW_Y_POSITION(ly));*/
+
+		if(lx > X_MIN_POS && lx <= X_MAX_POS && ly >= Y_MIN_POS && ly <= Y_MAX_POS) {
+			float templx = lx - (ly * planner.xy_skew_factor);
+			if(templx >= X_MIN_POS && templx <= X_MAX_POS) {
+				lx = templx;
+				//SERIAL_ECHOLNPAIR(" X(skewed): ", lx);
+			}
+		}
+	#endif
+
     #if ENABLED(AUTO_BED_LEVELING_UBL)
       if (!ubl.state.active) return;
       #if ENABLED(ENABLE_LEVELING_FADE_HEIGHT)
@@ -590,8 +610,8 @@ void Planner::check_axes_activity() {
     #endif
   }
 
-  void Planner::unapply_leveling(float logical[XYZ]) {
-
+  void Planner::unapply_leveling_old(float logical[XYZ]) {
+	
     #if ENABLED(AUTO_BED_LEVELING_UBL)
 
       if (ubl.state.active) {
@@ -671,6 +691,22 @@ void Planner::check_axes_activity() {
 
     #endif
   }
+
+  void Planner::unapply_leveling(float logical[XYZ]) {
+
+	  planner.unapply_leveling_old(logical);
+
+	  #if ENABLED(XY_SKEW_CORRECTION)
+	  	if(logical[X_AXIS] > X_MIN_POS && logical[X_AXIS] <= X_MAX_POS && logical[Y_AXIS] >= Y_MIN_POS && logical[Y_AXIS] <= Y_MAX_POS) {
+  			//float skew_offset = max(abs(planner.xy_skew_factor * Y_MIN_POS), abs(planner.xy_skew_factor * Y_MAX_POS));
+  			float templx = logical[X_AXIS] /*- skew_offset*/ + (logical[Y_AXIS] * planner.xy_skew_factor);
+  			if(templx >= X_MIN_POS && templx <= X_MAX_POS) {
+  				logical[X_AXIS] = templx;
+  				SERIAL_ECHOLNPAIR(" X(unskewed): ", logical[X_AXIS]);
+  			}
+		  }
+	  #endif
+   }
 
 #endif // PLANNER_LEVELING
 

Marlin/planner.h

@@ -168,6 +168,10 @@ class Planner {
     #if ENABLED(LIN_ADVANCE)
       static float extruder_advance_k, advance_ed_ratio;
     #endif
+
+	#if ENABLED(XY_SKEW_CORRECTION)
+      static float xy_skew_factor;
+    #endif
 
   private:
 
@@ -256,6 +260,7 @@ class Planner {
       static void apply_leveling(float &lx, float &ly, float &lz);
       static void apply_leveling(float logical[XYZ]) { apply_leveling(logical[X_AXIS], logical[Y_AXIS], logical[Z_AXIS]); }
       static void unapply_leveling(float logical[XYZ]);
+	    static void unapply_leveling_old(float logical[XYZ]);
 
     #else