🐛⚡️ FT_MOTION improvements (MarlinFirmware#26074)

Co-Authored-By: Scott Lahteine <[email protected]>

Author: narno2202

Marlin/Configuration_adv.h

@@ -1121,42 +1121,62 @@
 #if ENABLED(FT_MOTION)
   #define FTM_DEFAULT_MODE        ftMotionMode_DISABLED // Default mode of fixed time control. (Enums in ft_types.h)
   #define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (Enums in ft_types.h)
-  #define FTM_SHAPING_DEFAULT_X_FREQ 37.0f              // (Hz) Default peak frequency used by input shapers.
-  #define FTM_SHAPING_DEFAULT_Y_FREQ 37.0f              // (Hz) Default peak frequency used by input shapers.
-  #define FTM_LINEAR_ADV_DEFAULT_ENA false              // Default linear advance enable (true) or disable (false).
-  #define FTM_LINEAR_ADV_DEFAULT_K    0.0f              // Default linear advance gain.
-  #define FTM_SHAPING_ZETA            0.1f              // Zeta used by input shapers.
-  #define FTM_SHAPING_V_TOL           0.05f             // Vibration tolerance used by EI input shapers.
+  #define FTM_SHAPING_DEFAULT_X_FREQ   37.0f      // (Hz) Default peak frequency used by input shapers
+  #define FTM_SHAPING_DEFAULT_Y_FREQ   37.0f      // (Hz) Default peak frequency used by input shapers
+  #define FTM_LINEAR_ADV_DEFAULT_ENA   false      // Default linear advance enable (true) or disable (false)
+  #define FTM_LINEAR_ADV_DEFAULT_K      0.0f      // Default linear advance gain
+  #define FTM_SHAPING_ZETA_X            0.1f      // Zeta used by input shapers for X axis
+  #define FTM_SHAPING_ZETA_Y            0.1f      // Zeta used by input shapers for Y axis
+
+  #define FTM_SHAPING_V_TOL_X           0.05f     // Vibration tolerance used by EI input shapers for X axis
+  #define FTM_SHAPING_V_TOL_Y           0.05f     // Vibration tolerance used by EI input shapers for Y axis
+
+  //#define FT_MOTION_MENU                        // Provide a MarlinUI menu to set M493 parameters
 
   /**
    * Advanced configuration
    */
-  #define FTM_BATCH_SIZE            100                 // Batch size for trajectory generation;
-  #define FTM_WINDOW_SIZE           200                 // Window size for trajectory generation.
-  #define FTM_FS                   1000                 // (Hz) Frequency for trajectory generation. (1 / FTM_TS)
-  #define FTM_TS                      0.001f            // (s) Time step for trajectory generation. (1 / FTM_FS)
-  #define FTM_STEPPER_FS          20000                 // (Hz) Frequency for stepper I/O update.
-  #define FTM_MIN_TICKS ((STEPPER_TIMER_RATE) / (FTM_STEPPER_FS)) // Minimum stepper ticks between steps.
-  #define FTM_MIN_SHAPE_FREQ         10                 // Minimum shaping frequency.
-  #define FTM_ZMAX                  100                 // Maximum delays for shaping functions (even numbers only!).
-                                                        // Calculate as:
-                                                        //    1/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZV.
-                                                        //    (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZVD, MZV.
-                                                        //    3/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 2HEI.
-                                                        //    2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 3HEI.
-  #define FTM_STEPS_PER_UNIT_TIME    20                 // Interpolated stepper commands per unit time.
-                                                        // Calculate as (FTM_STEPPER_FS / FTM_FS).
-  #define FTM_CTS_COMPARE_VAL        10                 // Comparison value used in interpolation algorithm.
-                                                        // Calculate as (FTM_STEPS_PER_UNIT_TIME / 2).
-  // These values may be configured to adjust duration of loop().
-  #define FTM_STEPS_PER_LOOP         60                 // Number of stepper commands to generate each loop().
-  #define FTM_POINTS_PER_LOOP       100                 // Number of trajectory points to generate each loop().
-
-  // This value may be configured to adjust duration to consume the command buffer.
-  // Try increasing this value if stepper motion is not smooth.
-  #define FTM_STEPPERCMD_BUFF_SIZE 1000                 // Size of the stepper command buffers.
-
-  //#define FT_MOTION_MENU                              // Provide a MarlinUI menu to set M493 parameters.
+  #define FTM_UNIFIED_BWS                         // DON'T DISABLE unless you use Ulendo FBS (not implemented)
+  #if ENABLED(FTM_UNIFIED_BWS)
+    #define FTM_BW_SIZE               100         // Unified Window and Batch size with a ratio of 2
+  #else
+    #define FTM_WINDOW_SIZE           200         // Custom Window size for trajectory generation needed by Ulendo FBS
+    #define FTM_BATCH_SIZE            100         // Custom Batch size for trajectory generation needed by Ulendo FBS
+  #endif
+
+  #define FTM_FS                     1000         // (Hz) Frequency for trajectory generation. (Reciprocal of FTM_TS)
+  #define FTM_TS                        0.001f    // (s) Time step for trajectory generation. (Reciprocal of FTM_FS)
+
+  // These values may be configured to adjust the duration of loop().
+  #define FTM_STEPS_PER_LOOP           60         // Number of stepper commands to generate each loop()
+  #define FTM_POINTS_PER_LOOP         100         // Number of trajectory points to generate each loop()
+
+  #if DISABLED(COREXY)
+    #define FTM_STEPPER_FS          20000         // (Hz) Frequency for stepper I/O update
+
+    // Use this to adjust the time required to consume the command buffer.
+    // Try increasing this value if stepper motion is choppy.
+    #define FTM_STEPPERCMD_BUFF_SIZE 3000         // Size of the stepper command buffers
+                                                  // (FTM_STEPS_PER_LOOP * FTM_POINTS_PER_LOOP) is a good start
+                                                  // If you run out of memory, fall back to 3000 and increase progressively
+  #else
+    // CoreXY motion needs a larger buffer size. These values are based on our testing.
+    #define FTM_STEPPER_FS          30000
+    #define FTM_STEPPERCMD_BUFF_SIZE 6000
+  #endif
+
+  #define FTM_STEPS_PER_UNIT_TIME (FTM_STEPPER_FS / FTM_FS)       // Interpolated stepper commands per unit time
+  #define FTM_CTS_COMPARE_VAL (FTM_STEPS_PER_UNIT_TIME / 2)       // Comparison value used in interpolation algorithm
+  #define FTM_MIN_TICKS ((STEPPER_TIMER_RATE) / (FTM_STEPPER_FS)) // Minimum stepper ticks between steps
+
+  #define FTM_MIN_SHAPE_FREQ           10         // Minimum shaping frequency
+  #define FTM_RATIO (FTM_FS / FTM_MIN_SHAPE_FREQ) // Factor for use in FTM_ZMAX. DON'T CHANGE.
+  #define FTM_ZMAX (FTM_RATIO * 2)                // Maximum delays for shaping functions (even numbers only!)
+                                                  // Calculate as:
+                                                  //   ZV       : FTM_RATIO / 2
+                                                  //   ZVD, MZV : FTM_RATIO
+                                                  //   2HEI     : FTM_RATIO * 3 / 2
+                                                  //   3HEI     : FTM_RATIO * 2
 #endif
 
 /**

Marlin/src/gcode/bedlevel/G35.cpp

@@ -57,9 +57,9 @@
  *               41 - Counter-Clockwise M4
  *               50 - Clockwise M5
  *               51 - Counter-Clockwise M5
- *
- */
+ **/
 void GcodeSuite::G35() {
+
   DEBUG_SECTION(log_G35, "G35", DEBUGGING(LEVELING));
 
   if (DEBUGGING(LEVELING)) log_machine_info();

Marlin/src/gcode/bedlevel/abl/G29.cpp

@@ -225,6 +225,7 @@ class G29_State {
  *     There's no extra effect if you have a fixed Z probe.
  */
 G29_TYPE GcodeSuite::G29() {
+
   DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING));
 
   // Leveling state is persistent when done manually with multiple G29 commands

Marlin/src/gcode/bedlevel/mbl/G29.cpp

@@ -64,6 +64,7 @@ inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM("
  *  S5              Reset and disable mesh
  */
 void GcodeSuite::G29() {
+
   DEBUG_SECTION(log_G29, "G29", true);
 
   // G29 Q is also available if debugging

Marlin/src/gcode/calibrate/G34_M422.cpp

@@ -79,6 +79,7 @@
  *   R                 Flag to recalculate points based on current probe offsets
  */
 void GcodeSuite::G34() {
+
   DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING));
   if (DEBUGGING(LEVELING)) log_machine_info();
 

Marlin/src/gcode/feature/ft_motion/M493.cpp

@@ -26,6 +26,7 @@
 
 #include "../../gcode.h"
 #include "../../../module/ft_motion.h"
+#include "../../../module/stepper.h"
 
 void say_shaping() {
   // FT Enabled
@@ -39,6 +40,8 @@ void say_shaping() {
         default: break;
         case ftMotionMode_ZV:   SERIAL_ECHOPGM("ZV");        break;
         case ftMotionMode_ZVD:  SERIAL_ECHOPGM("ZVD");       break;
+        case ftMotionMode_ZVDD: SERIAL_ECHOPGM("ZVDD");      break;
+        case ftMotionMode_ZVDDD: SERIAL_ECHOPGM("ZVDDD");    break;
         case ftMotionMode_EI:   SERIAL_ECHOPGM("EI");        break;
         case ftMotionMode_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break;
         case ftMotionMode_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break;
@@ -155,20 +158,19 @@ void GcodeSuite::M493() {
 
   if (!parser.seen_any())
     flag.report_h = true;
-  else
-    planner.synchronize();
 
   // Parse 'S' mode parameter.
   if (parser.seenval('S')) {
-    const ftMotionMode_t oldmm = ftMotion.cfg.mode,
-                         newmm = (ftMotionMode_t)parser.value_byte();
+    const ftMotionMode_t newmm = (ftMotionMode_t)parser.value_byte();
 
-    if (newmm != oldmm) {
+    if (newmm != ftMotion.cfg.mode) {
       switch (newmm) {
         default: SERIAL_ECHOLNPGM("?Invalid control mode [S] value."); return;
         #if HAS_X_AXIS
           case ftMotionMode_ZV:
           case ftMotionMode_ZVD:
+          case ftMotionMode_ZVDD:
+          case ftMotionMode_ZVDDD:
           case ftMotionMode_EI:
           case ftMotionMode_2HEI:
           case ftMotionMode_3HEI:
@@ -177,11 +179,10 @@ void GcodeSuite::M493() {
           //case ftMotionMode_DISCTF:
             flag.update_n = flag.update_a = true;
         #endif
-        case ftMotionMode_DISABLED:
+        case ftMotionMode_DISABLED: flag.reset_ft = true;
         case ftMotionMode_ENABLED:
           ftMotion.cfg.mode = newmm;
           flag.report_h = true;
-          if (oldmm == ftMotionMode_DISABLED) flag.reset_ft = true;
           break;
       }
     }
@@ -193,6 +194,7 @@ void GcodeSuite::M493() {
     if (parser.seen('P')) {
       const bool val = parser.value_bool();
       ftMotion.cfg.linearAdvEna = val;
+      flag.report_h = true;
       SERIAL_ECHO_TERNARY(val, "Linear Advance ", "en", "dis", "abled.\n");
     }
 
@@ -216,22 +218,16 @@ void GcodeSuite::M493() {
       if (ftMotion.cfg.modeHasShaper()) {
         const dynFreqMode_t val = dynFreqMode_t(parser.value_byte());
         switch (val) {
-          case dynFreqMode_DISABLED:
-            ftMotion.cfg.dynFreqMode = val;
-            flag.report_h = true;
-            break;
           #if HAS_DYNAMIC_FREQ_MM
             case dynFreqMode_Z_BASED:
-              ftMotion.cfg.dynFreqMode = val;
-              flag.report_h = true;
-              break;
           #endif
           #if HAS_DYNAMIC_FREQ_G
             case dynFreqMode_MASS_BASED:
-              ftMotion.cfg.dynFreqMode = val;
-              flag.report_h = true;
-              break;
           #endif
+          case dynFreqMode_DISABLED:
+            ftMotion.cfg.dynFreqMode = val;
+            flag.report_h = true;
+            break;
           default:
             SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value.");
             break;
@@ -279,6 +275,36 @@ void GcodeSuite::M493() {
       }
     #endif
 
+    // Parse zeta parameter (X axis).
+    if (parser.seenval('I')) {
+      const float val = parser.value_float();
+      if (ftMotion.cfg.modeHasShaper()) {
+        if (WITHIN(val, 0.01f, 1.0f)) {
+          ftMotion.cfg.zeta[0] = val;
+          flag.update_n = flag.update_a = true;
+        }
+        else
+          SERIAL_ECHOLNPGM("Invalid X zeta [", AS_CHAR('I'), "] value."); // Zeta out of range.
+      }
+      else
+        SERIAL_ECHOLNPGM("Wrong mode for zeta parameter.");
+    }
+
+    // Parse vtol parameter (X axis).
+    if (parser.seenval('Q')) {
+      const float val = parser.value_float();
+      if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) {
+        if (WITHIN(val, 0.00f, 1.0f)) {
+          ftMotion.cfg.vtol[0] = val;
+          flag.update_a = true;
+        }
+        else
+          SERIAL_ECHOLNPGM("Invalid X vtol [", AS_CHAR('Q'), "] value."); // VTol out of range.
+      }
+      else
+        SERIAL_ECHOLNPGM("Wrong mode for vtol parameter.");
+    }
+
   #endif // HAS_X_AXIS
 
   #if HAS_Y_AXIS
@@ -310,15 +336,50 @@ void GcodeSuite::M493() {
       }
     #endif
 
+    // Parse zeta parameter (Y axis).
+    if (parser.seenval('J')) {
+      const float val = parser.value_float();
+      if (ftMotion.cfg.modeHasShaper()) {
+        if (WITHIN(val, 0.01f, 1.0f)) {
+          ftMotion.cfg.zeta[1] = val;
+          flag.update_n = flag.update_a = true;
+        }
+        else
+          SERIAL_ECHOLNPGM("Invalid Y zeta [", AS_CHAR('J'), "] value."); // Zeta Out of range
+      }
+      else
+        SERIAL_ECHOLNPGM("Wrong mode for zeta parameter.");
+    }
+
+    // Parse vtol parameter (Y axis).
+    if (parser.seenval('R')) {
+      const float val = parser.value_float();
+      if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) {
+        if (WITHIN(val, 0.00f, 1.0f)) {
+          ftMotion.cfg.vtol[1] = val;
+          flag.update_a = true;
+        }
+        else
+          SERIAL_ECHOLNPGM("Invalid Y vtol [", AS_CHAR('R'), "] value."); // VTol out of range.
+      }
+      else
+        SERIAL_ECHOLNPGM("Wrong mode for vtol parameter.");
+    }
+
   #endif // HAS_Y_AXIS
 
-  #if HAS_X_AXIS
-    if (flag.update_n) ftMotion.refreshShapingN();
-    if (flag.update_a) ftMotion.updateShapingA();
-  #endif
-  if (flag.reset_ft) ftMotion.reset();
-  if (flag.report_h) say_shaping();
+  planner.synchronize();
 
+  if (flag.update_n) ftMotion.refreshShapingN();
+
+  if (flag.update_a) ftMotion.updateShapingA();
+
+  if (flag.reset_ft) {
+    stepper.ftMotion_syncPosition();
+    ftMotion.reset();
+  }
+
+  if (flag.report_h) say_shaping();
 }
 
 #endif // FT_MOTION

Marlin/src/gcode/probe/G38.cpp

@@ -105,6 +105,7 @@ inline bool G38_run_probe() {
  *  G38.5 - Probe away from workpiece, stop on contact break
  */
 void GcodeSuite::G38(const int8_t subcode) {
+
   // Get X Y Z E F
   get_destination_from_command();
 

Marlin/src/inc/Conditionals_adv.h

@@ -1144,7 +1144,7 @@
   #elif HAS_DRIVER(A4988)
     #define MINIMUM_STEPPER_POST_DIR_DELAY 200
   #elif HAS_TRINAMIC_CONFIG || HAS_TRINAMIC_STANDALONE
-    #define MINIMUM_STEPPER_POST_DIR_DELAY 60
+    #define MINIMUM_STEPPER_POST_DIR_DELAY 70
   #else
     #define MINIMUM_STEPPER_POST_DIR_DELAY 0   // Expect at least 10µS since one Stepper ISR must transpire
   #endif

Marlin/src/lcd/language/language_en.h

@@ -802,6 +802,8 @@ namespace LanguageNarrow_en {
   LSTR MSG_FTM_MODE                       = _UxGT("Motion Mode:");
   LSTR MSG_FTM_ZV                         = _UxGT("ZV");
   LSTR MSG_FTM_ZVD                        = _UxGT("ZVD");
+  LSTR MSG_FTM_ZVDD                       = _UxGT("ZVDD");
+  LSTR MSG_FTM_ZVDDD                      = _UxGT("ZVDDD");
   LSTR MSG_FTM_EI                         = _UxGT("EI");
   LSTR MSG_FTM_2HEI                       = _UxGT("2HEI");
   LSTR MSG_FTM_3HEI                       = _UxGT("3HEI");
@@ -813,8 +815,8 @@ namespace LanguageNarrow_en {
   LSTR MSG_FTM_MASS_BASED                 = _UxGT("Mass-based");
   LSTR MSG_FTM_BASE_FREQ_N                = _UxGT("@ Base Freq.");
   LSTR MSG_FTM_DFREQ_K_N                  = _UxGT("@ Dyn. Freq.");
-  LSTR MSG_FTM_ZETA                       = _UxGT("Damping");
-  LSTR MSG_FTM_VTOL                       = _UxGT("Vib. Level");
+  LSTR MSG_FTM_ZETA_N                     = _UxGT("@ Damping");
+  LSTR MSG_FTM_VTOL_N                     = _UxGT("@ Vib. Level");
 
   LSTR MSG_LEVEL_X_AXIS                   = _UxGT("Level X Axis");
   LSTR MSG_AUTO_CALIBRATE                 = _UxGT("Auto Calibrate");