🚀 ZV Input Shaping (#24797)

Marlin/Configuration_adv.h

 
 // @section motion
 
+/**
+ * Input Shaping -- EXPERIMENTAL
+ *
+ * Zero Vibration (ZV) Input Shaping for X and/or Y movements.
+ *
+ * This option uses a lot of SRAM for the step buffer, which is proportional
+ * to the largest step rate possible for any axis. If the build fails due to
+ * low SRAM the buffer size may be reduced by setting smaller values for
+ * DEFAULT_AXIS_STEPS_PER_UNIT and/or DEFAULT_MAX_FEEDRATE. Runtime editing
+ * of max feedrate (M203) or resonant frequency (M593) may result feedrate
+ * being capped to prevent buffer overruns.
+ *
+ * Tune with M593 D<factor> F<frequency>:
+ *
+ *  D<factor>    Set the zeta/damping factor. If axes (X, Y, etc.) are not specified, set for all axes.
+ *  F<frequency> Set the frequency. If axes (X, Y, etc.) are not specified, set for all axes.
+ *  T[map]       Input Shaping type, 0:ZV, 1:EI, 2:2H EI (not implemented yet)
+ *  X<1>         Set the given parameters only for the X axis.
+ *  Y<1>         Set the given parameters only for the Y axis.
+ */
+//#define INPUT_SHAPING
+#if ENABLED(INPUT_SHAPING)
+  #define SHAPING_FREQ_X    40  // (Hz) The dominant resonant frequency of the X axis.
+  #define SHAPING_FREQ_Y    40  // (Hz) The dominant resonant frequency of the Y axis.
+  #define SHAPING_ZETA_X  0.3f  // Damping ratio of the X axis (range: 0.0 = no damping to 1.0 = critical damping).
+  #define SHAPING_ZETA_Y  0.3f  // Damping ratio of the Y axis (range: 0.0 = no damping to 1.0 = critical damping).
+  //#define SHAPING_MENU        // Add a menu to the LCD to set shaping parameters.
+#endif
+
 #define AXIS_RELATIVE_MODES { false, false, false, false }
 
 // Add a Duplicate option for well-separated conjoined nozzles

Marlin/src/gcode/feature/input_shaping/M593.cpp

+/**
+ * Marlin 3D Printer Firmware
+ * Copyright (c) 2022 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
+ *
+ * Based on Sprinter and grbl.
+ * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "../../../inc/MarlinConfig.h"
+
+#if ENABLED(INPUT_SHAPING)
+
+#include "../../gcode.h"
+#include "../../../module/stepper.h"
+
+void GcodeSuite::M593_report(const bool forReplay/*=true*/) {
+  report_heading_etc(forReplay, F("Input Shaping"));
+  #if HAS_SHAPING_X
+    SERIAL_ECHO_MSG("M593 X"
+      " F", stepper.get_shaping_frequency(X_AXIS),
+      " D", stepper.get_shaping_damping_ratio(X_AXIS)
+    );
+  #endif
+  #if HAS_SHAPING_Y
+    SERIAL_ECHO_MSG("M593 Y"
+      " F", stepper.get_shaping_frequency(Y_AXIS),
+      " D", stepper.get_shaping_damping_ratio(Y_AXIS)
+    );
+  #endif
+}
+
+/**
+ * M593: Get or Set Input Shaping Parameters
+ *  D<factor>    Set the zeta/damping factor. If axes (X, Y, etc.) are not specified, set for all axes.
+ *  F<frequency> Set the frequency. If axes (X, Y, etc.) are not specified, set for all axes.
+ *  T[map]       Input Shaping type, 0:ZV, 1:EI, 2:2H EI (not implemented yet)
+ *  X<1>         Set the given parameters only for the X axis.
+ *  Y<1>         Set the given parameters only for the Y axis.
+ */
+void GcodeSuite::M593() {
+  if (!parser.seen_any()) return M593_report();
+
+  const bool seen_X = TERN0(HAS_SHAPING_X, parser.seen_test('X')),
+             seen_Y = TERN0(HAS_SHAPING_Y, parser.seen_test('Y')),
+             for_X = seen_X || TERN0(HAS_SHAPING_X, (!seen_X && !seen_Y)),
+             for_Y = seen_Y || TERN0(HAS_SHAPING_Y, (!seen_X && !seen_Y));
+
+  if (parser.seen('D')) {
+    const float zeta = parser.value_float();
+    if (WITHIN(zeta, 0, 1)) {
+      if (for_X) stepper.set_shaping_damping_ratio(X_AXIS, zeta);
+      if (for_Y) stepper.set_shaping_damping_ratio(Y_AXIS, zeta);
+    }
+    else
+      SERIAL_ECHO_MSG("?Zeta (D) value out of range (0-1)");
+  }
+
+  if (parser.seen('F')) {
+    const float freq = parser.value_float();
+    if (freq > 0) {
+      if (for_X) stepper.set_shaping_frequency(X_AXIS, freq);
+      if (for_Y) stepper.set_shaping_frequency(Y_AXIS, freq);
+    }
+    else
+      SERIAL_ECHO_MSG("?Frequency (F) must be greater than 0");
+  }
+}
+
+#endif

Marlin/src/gcode/gcode.cpp

         case 575: M575(); break;                                  // M575: Set serial baudrate
       #endif
 
+      #if ENABLED(INPUT_SHAPING)
+        case 593: M593(); break;                                  // M593: Set Input Shaping parameters
+      #endif
+
       #if ENABLED(ADVANCED_PAUSE_FEATURE)
         case 600: M600(); break;                                  // M600: Pause for Filament Change
         case 603: M603(); break;                                  // M603: Configure Filament Change

Marlin/src/gcode/gcode.h

  * M554 - Get or set IP gateway. (Requires enabled Ethernet port)
  * M569 - Enable stealthChop on an axis. (Requires at least one _DRIVER_TYPE to be TMC2130/2160/2208/2209/5130/5160)
  * M575 - Change the serial baud rate. (Requires BAUD_RATE_GCODE)
+ * M593 - Get or set input shaping parameters. (Requires INPUT_SHAPING)
  * M600 - Pause for filament change: "M600 X<pos> Y<pos> Z<raise> E<first_retract> L<later_retract>". (Requires ADVANCED_PAUSE_FEATURE)
  * M603 - Configure filament change: "M603 T<tool> U<unload_length> L<load_length>". (Requires ADVANCED_PAUSE_FEATURE)
  * M605 - Set Dual X-Carriage movement mode: "M605 S<mode> [X<x_offset>] [R<temp_offset>]". (Requires DUAL_X_CARRIAGE)
     static void M575();
   #endif
 
+  #if ENABLED(INPUT_SHAPING)
+    static void M593();
+    static void M593_report(const bool forReplay=true);
+  #endif
+
   #if ENABLED(ADVANCED_PAUSE_FEATURE)
     static void M600();
     static void M603();

Marlin/src/inc/Conditionals_adv.h

 #else
   #define CALC_FAN_SPEED(f) (f ? map(f, 1, 255, FAN_MIN_PWM, FAN_MAX_PWM) : FAN_OFF_PWM)
 #endif
+
+// Input shaping
+#if ENABLED(INPUT_SHAPING)
+  #if !HAS_Y_AXIS
+    #undef SHAPING_FREQ_Y
+    #undef SHAPING_BUFFER_Y
+  #endif
+  #ifdef SHAPING_FREQ_X
+    #define HAS_SHAPING_X 1
+  #endif
+  #ifdef SHAPING_FREQ_Y
+    #define HAS_SHAPING_Y 1
+  #endif
+#endif

Marlin/src/inc/SanityCheck.h

   #endif
 #endif
 
-// Misc. Cleanup
-#undef _TEST_PWM
-#undef _NUM_AXES_STR
-#undef _LOGICAL_AXES_STR
-
 // JTAG support in the HAL
 #if ENABLED(DISABLE_DEBUG) && !defined(JTAGSWD_DISABLE)
   #error "DISABLE_DEBUG is not supported for the selected MCU/Board."
 #if ENABLED(XFER_BUILD) && !BOTH(BINARY_FILE_TRANSFER, CUSTOM_FIRMWARE_UPLOAD)
   #error "BINARY_FILE_TRANSFER and CUSTOM_FIRMWARE_UPLOAD are required for custom upload."
 #endif
+
+// Check requirements for Input Shaping
+#if ENABLED(INPUT_SHAPING) && defined(__AVR__)
+  #if HAS_SHAPING_X
+    #if F_CPU > 16000000
+      static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (20) for AVR 20MHz.");
+    #else
+      static_assert((SHAPING_FREQ_X) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_X is below the minimum (16) for AVR 16MHz.");
+    #endif
+  #elif HAS_SHAPING_Y
+    #if F_CPU > 16000000
+      static_assert((SHAPING_FREQ_Y) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Y is below the minimum (20) for AVR 20MHz.");
+    #else
+      static_assert((SHAPING_FREQ_Y) * 2 * 0x10000 >= (STEPPER_TIMER_RATE), "SHAPING_FREQ_Y is below the minimum (16) for AVR 16MHz.");
+    #endif
+  #endif
+#endif
+
+#if ENABLED(INPUT_SHAPING)
+  #if ENABLED(DIRECT_STEPPING)
+    #error "INPUT_SHAPING cannot currently be used with DIRECT_STEPPING."
+  #elif ENABLED(LASER_FEATURE)
+    #error "INPUT_SHAPING cannot currently be used with LASER_FEATURE."
+  #endif
+#endif
+
+// Misc. Cleanup
+#undef _TEST_PWM
+#undef _NUM_AXES_STR
+#undef _LOGICAL_AXES_STR

Marlin/src/lcd/language/language_en.h

   LSTR MSG_AMAX_EN                        = _UxGT("Max * Accel");
   LSTR MSG_A_RETRACT                      = _UxGT("Retract Accel");
   LSTR MSG_A_TRAVEL                       = _UxGT("Travel Accel");
+  LSTR MSG_INPUT_SHAPING                  = _UxGT("Input Shaping");
+  LSTR MSG_SHAPING_X_FREQ                 = STR_X _UxGT(" frequency");
+  LSTR MSG_SHAPING_Y_FREQ                 = STR_Y _UxGT(" frequency");
+  LSTR MSG_SHAPING_X_ZETA                 = STR_X _UxGT(" damping");
+  LSTR MSG_SHAPING_Y_ZETA                 = STR_Y _UxGT(" damping");
   LSTR MSG_XY_FREQUENCY_LIMIT             = _UxGT("XY Freq Limit");
   LSTR MSG_XY_FREQUENCY_FEEDRATE          = _UxGT("Min FR Factor");
   LSTR MSG_STEPS_PER_MM                   = _UxGT("Steps/mm");

Marlin/src/lcd/menu/menu_advanced.cpp

 #include "menu_item.h"
 #include "../../MarlinCore.h"
 #include "../../module/planner.h"
+#include "../../module/stepper.h"
 
 #if DISABLED(NO_VOLUMETRICS)
   #include "../../gcode/parser.h"
 
 #if HAS_MOTOR_CURRENT_PWM
 
-  #include "../../module/stepper.h"
-
   void menu_pwm() {
     START_MENU();
     BACK_ITEM(MSG_ADVANCED_SETTINGS);
     END_MENU();
   }
 
+  #if ENABLED(SHAPING_MENU)
+
+    void menu_advanced_input_shaping() {
+      constexpr float min_frequency = TERN(__AVR__, float(STEPPER_TIMER_RATE) / 2 / 0x10000, 1.0f);
+
+      START_MENU();
+      BACK_ITEM(MSG_ADVANCED_SETTINGS);
+
+      // M593 F Frequency
+      #if HAS_SHAPING_X
+        editable.decimal = stepper.get_shaping_frequency(X_AXIS);
+        EDIT_ITEM_FAST(float61, MSG_SHAPING_X_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(X_AXIS, editable.decimal); });
+      #endif
+      #if HAS_SHAPING_Y
+        editable.decimal = stepper.get_shaping_frequency(Y_AXIS);
+        EDIT_ITEM_FAST(float61, MSG_SHAPING_Y_FREQ, &editable.decimal, min_frequency, 200.0f, []{ stepper.set_shaping_frequency(Y_AXIS, editable.decimal); });
+      #endif
+
+      // M593 D Damping ratio
+      #if HAS_SHAPING_X
+        editable.decimal = stepper.get_shaping_damping_ratio(X_AXIS);
+        EDIT_ITEM_FAST(float42_52, MSG_SHAPING_X_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(X_AXIS, editable.decimal); });
+      #endif
+      #if HAS_SHAPING_Y
+        editable.decimal = stepper.get_shaping_damping_ratio(Y_AXIS);
+        EDIT_ITEM_FAST(float42_52, MSG_SHAPING_Y_ZETA, &editable.decimal, 0.0f, 1.0f, []{ stepper.set_shaping_damping_ratio(Y_AXIS, editable.decimal); });
+      #endif
+
+      END_MENU();
+    }
+
+  #endif
+
   #if HAS_CLASSIC_JERK
 
     void menu_advanced_jerk() {
     // M201 - Acceleration items
     SUBMENU(MSG_ACCELERATION, menu_advanced_acceleration);
 
+    // M593 - Acceleration items
+    #if ENABLED(SHAPING_MENU)
+      SUBMENU(MSG_INPUT_SHAPING, menu_advanced_input_shaping);
+    #endif
+
     #if HAS_CLASSIC_JERK
       // M205 - Max Jerk
       SUBMENU(MSG_JERK, menu_advanced_jerk);

Marlin/src/module/planner.cpp

 
   #endif // XY_FREQUENCY_LIMIT
 
+  #if ENABLED(INPUT_SHAPING)
+    const float top_freq = _MIN(float(0x7FFFFFFFL)
+                                OPTARG(HAS_SHAPING_X, stepper.get_shaping_frequency(X_AXIS))
+                                OPTARG(HAS_SHAPING_Y, stepper.get_shaping_frequency(Y_AXIS))),
+                max_factor = (top_freq * float(shaping_dividends - 3) * 2.0f) / block->nominal_rate;
+    NOMORE(speed_factor, max_factor);
+  #endif
+
   // Correct the speed
   if (speed_factor < 1.0f) {
     current_speed *= speed_factor;

Marlin/src/module/settings.cpp

     MPC_t mpc_constants[HOTENDS];                       // M306
   #endif
 
+  //
+  // Input Shaping
+  //
+  #if HAS_SHAPING_X
+    float shaping_x_frequency, // M593 X F
+          shaping_x_zeta;      // M593 X D
+  #endif
+  #if HAS_SHAPING_Y
+    float shaping_y_frequency, // M593 Y F
+          shaping_y_zeta;      // M593 Y D
+  #endif
+
 } SettingsData;
 
 //static_assert(sizeof(SettingsData) <= MARLIN_EEPROM_SIZE, "EEPROM too small to contain SettingsData!");
     #endif
 
     //
+    // Input Shaping
+    ///
+    #if ENABLED(INPUT_SHAPING)
+      #if HAS_SHAPING_X
+        EEPROM_WRITE(stepper.get_shaping_frequency(X_AXIS));
+        EEPROM_WRITE(stepper.get_shaping_damping_ratio(X_AXIS));
+      #endif
+      #if HAS_SHAPING_Y
+        EEPROM_WRITE(stepper.get_shaping_frequency(Y_AXIS));
+        EEPROM_WRITE(stepper.get_shaping_damping_ratio(Y_AXIS));
+      #endif
+    #endif
+
+    //
     // Report final CRC and Data Size
     //
     if (!eeprom_error) {
       #endif
 
       //
+      // Input Shaping
+      //
+      #if HAS_SHAPING_X
+      {
+        float _data[2];
+        EEPROM_READ(_data);
+        stepper.set_shaping_frequency(X_AXIS, _data[0]);
+        stepper.set_shaping_damping_ratio(X_AXIS, _data[1]);
+      }
+      #endif
+
+      #if HAS_SHAPING_Y
+      {
+        float _data[2];
+        EEPROM_READ(_data);
+        stepper.set_shaping_frequency(Y_AXIS, _data[0]);
+        stepper.set_shaping_damping_ratio(Y_AXIS, _data[1]);
+      }
+      #endif
+
+      //
       // Validate Final Size and CRC
       //
       eeprom_error = size_error(eeprom_index - (EEPROM_OFFSET));
     }
   #endif
 
+  //
+  // Input Shaping
+  //
+  #if ENABLED(INPUT_SHAPING)
+    #if HAS_SHAPING_X
+      stepper.set_shaping_frequency(X_AXIS, SHAPING_FREQ_X);
+      stepper.set_shaping_damping_ratio(X_AXIS, SHAPING_ZETA_X);
+    #endif
+    #if HAS_SHAPING_Y
+      stepper.set_shaping_frequency(Y_AXIS, SHAPING_FREQ_Y);
+      stepper.set_shaping_damping_ratio(Y_AXIS, SHAPING_ZETA_Y);
+    #endif
+  #endif
+
   postprocess();
 
   #if EITHER(EEPROM_CHITCHAT, DEBUG_LEVELING_FEATURE)
     TERN_(HAS_STEALTHCHOP, gcode.M569_report(forReplay));
 
     //
+    // Input Shaping
+    //
+    TERN_(INPUT_SHAPING, gcode.M593_report(forReplay));
+
+    //
     // Linear Advance
     //
     TERN_(LIN_ADVANCE, gcode.M900_report(forReplay));

Marlin/src/module/stepper.cpp

 
 xyze_long_t Stepper::delta_error{0};
 
-xyze_ulong_t Stepper::advance_dividend{0};
+xyze_long_t Stepper::advance_dividend{0};
 uint32_t Stepper::advance_divisor = 0,
          Stepper::step_events_completed = 0, // The number of step events executed in the current block
          Stepper::accelerate_until,          // The count at which to stop accelerating
            Stepper::la_advance_steps = 0;
 #endif
 
+#if ENABLED(INPUT_SHAPING)
+  shaping_time_t                DelayTimeManager::now = 0;
+  ParamDelayQueue               Stepper::shaping_dividend_queue;
+  DelayQueue<shaping_dividends> Stepper::shaping_queue;
+  #if HAS_SHAPING_X
+    shaping_time_t DelayTimeManager::delay_x;
+    ShapeParams Stepper::shaping_x;
+  #endif
+  #if HAS_SHAPING_Y
+    shaping_time_t DelayTimeManager::delay_y;
+    ShapeParams Stepper::shaping_y;
+  #endif
+#endif
+
 #if ENABLED(INTEGRATED_BABYSTEPPING)
   uint32_t Stepper::nextBabystepISR = BABYSTEP_NEVER;
 #endif
 #define PULSE_LOW_TICK_COUNT hal_timer_t(NS_TO_PULSE_TIMER_TICKS(_MIN_PULSE_LOW_NS - _MIN(_MIN_PULSE_LOW_NS, TIMER_SETUP_NS)))
 
 #define USING_TIMED_PULSE() hal_timer_t start_pulse_count = 0
-#define START_TIMED_PULSE(DIR) (start_pulse_count = HAL_timer_get_count(MF_TIMER_PULSE))
-#define AWAIT_TIMED_PULSE(DIR) while (PULSE_##DIR##_TICK_COUNT > HAL_timer_get_count(MF_TIMER_PULSE) - start_pulse_count) { }
-#define START_HIGH_PULSE()  START_TIMED_PULSE(HIGH)
-#define AWAIT_HIGH_PULSE()  AWAIT_TIMED_PULSE(HIGH)
-#define START_LOW_PULSE()   START_TIMED_PULSE(LOW)
-#define AWAIT_LOW_PULSE()   AWAIT_TIMED_PULSE(LOW)
+#define START_TIMED_PULSE() (start_pulse_count = HAL_timer_get_count(MF_TIMER_PULSE))
+#define AWAIT_TIMED_PULSE(DIR) while (PULSE_##DIR##_TICK_COUNT > HAL_timer_get_count(MF_TIMER_PULSE) - start_pulse_count) { /* nada */ }
+#define AWAIT_HIGH_PULSE() AWAIT_TIMED_PULSE(HIGH)
+#define AWAIT_LOW_PULSE()  AWAIT_TIMED_PULSE(LOW)
 
 #if MINIMUM_STEPPER_PRE_DIR_DELAY > 0
   #define DIR_WAIT_BEFORE() DELAY_NS(MINIMUM_STEPPER_PRE_DIR_DELAY)
   TERN_(EXTENSIBLE_UI, ExtUI::onSteppersDisabled());
 }
 
+#define SET_STEP_DIR(A)                       \
+  if (motor_direction(_AXIS(A))) {            \
+    A##_APPLY_DIR(INVERT_##A##_DIR, false);   \
+    count_direction[_AXIS(A)] = -1;           \
+  }                                           \
+  else {                                      \
+    A##_APPLY_DIR(!INVERT_##A##_DIR, false);  \
+    count_direction[_AXIS(A)] = 1;            \
+  }
+
 /**
  * Set the stepper direction of each axis
  *
 
   DIR_WAIT_BEFORE();
 
-  #define SET_STEP_DIR(A)                       \
-    if (motor_direction(_AXIS(A))) {            \
-      A##_APPLY_DIR(INVERT_##A##_DIR, false);   \
-      count_direction[_AXIS(A)] = -1;           \
-    }                                           \
-    else {                                      \
-      A##_APPLY_DIR(!INVERT_##A##_DIR, false);  \
-      count_direction[_AXIS(A)] = 1;            \
-    }
-
   TERN_(HAS_X_DIR, SET_STEP_DIR(X)); // A
   TERN_(HAS_Y_DIR, SET_STEP_DIR(Y)); // B
   TERN_(HAS_Z_DIR, SET_STEP_DIR(Z)); // C
     // Enable ISRs to reduce USART processing latency
     hal.isr_on();
 
+    #if ENABLED(INPUT_SHAPING)
+      // Speed limiting should ensure the buffers never get full. But if somehow they do, stutter rather than overflow.
+      if (!nextMainISR) {
+        TERN_(HAS_SHAPING_X, if (shaping_dividend_queue.free_count_x() == 0) nextMainISR = shaping_dividend_queue.peek_x() + 1);
+        TERN_(HAS_SHAPING_Y, if (shaping_dividend_queue.free_count_y() == 0) NOLESS(nextMainISR, shaping_dividend_queue.peek_y() + 1));
+        TERN_(HAS_SHAPING_X, if (shaping_queue.free_count_x() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_x() + 1));
+        TERN_(HAS_SHAPING_Y, if (shaping_queue.free_count_y() < steps_per_isr) NOLESS(nextMainISR, shaping_queue.peek_y() + 1));
+      }
+    #endif
+
     if (!nextMainISR) pulse_phase_isr();                // 0 = Do coordinated axes Stepper pulses
 
+    TERN_(INPUT_SHAPING, shaping_isr());                // Do Shaper stepping, if needed
+
     #if ENABLED(LIN_ADVANCE)
       if (!nextAdvanceISR) {                            // 0 = Do Linear Advance E Stepper pulses
         advance_isr();
 
     // Get the interval to the next ISR call
     const uint32_t interval = _MIN(
-      uint32_t(HAL_TIMER_TYPE_MAX),                     // Come back in a very long time
-      nextMainISR                                       // Time until the next Pulse / Block phase
-      OPTARG(LIN_ADVANCE, nextAdvanceISR)               // Come back early for Linear Advance?
-      OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR)  // Come back early for Babystepping?
+      uint32_t(HAL_TIMER_TYPE_MAX),                           // Come back in a very long time
+      nextMainISR                                             // Time until the next Pulse / Block phase
+      OPTARG(HAS_SHAPING_X, shaping_dividend_queue.peek_x())  // Time until next input shaping dividend change for X
+      OPTARG(HAS_SHAPING_Y, shaping_dividend_queue.peek_y())  // Time until next input shaping dividend change for Y
+      OPTARG(HAS_SHAPING_X, shaping_queue.peek_x())           // Time until next input shaping echo for X
+      OPTARG(HAS_SHAPING_Y, shaping_queue.peek_y())           // Time until next input shaping echo for Y
+      OPTARG(LIN_ADVANCE, nextAdvanceISR)                     // Come back early for Linear Advance?
+      OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR)        // Come back early for Babystepping?
     );
 
     //
 
     nextMainISR -= interval;
 
+    TERN_(INPUT_SHAPING, DelayTimeManager::decrement_delays(interval));
+
     #if ENABLED(LIN_ADVANCE)
       if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval;
     #endif
   // If we must abort the current block, do so!
   if (abort_current_block) {
     abort_current_block = false;
-    if (current_block) discard_current_block();
+    if (current_block) {
+      discard_current_block();
+      #if ENABLED(INPUT_SHAPING)
+        shaping_dividend_queue.purge();
+        shaping_queue.purge();
+        TERN_(HAS_SHAPING_X, delta_error.x = 0);
+        TERN_(HAS_SHAPING_Y, delta_error.y = 0);
+      #endif
+    }
   }
 
   // If there is no current block, do nothing
-  if (!current_block) return;
+  if (!current_block || step_events_completed >= step_event_count) return;
 
   // Skipping step processing causes motion to freeze
   if (TERN0(FREEZE_FEATURE, frozen)) return;
   #endif
   xyze_bool_t step_needed{0};
 
+  // Direct Stepping page?
+  const bool is_page = current_block->is_page();
+
   do {
     #define _APPLY_STEP(AXIS, INV, ALWAYS) AXIS ##_APPLY_STEP(INV, ALWAYS)
     #define _INVERT_STEP_PIN(AXIS) INVERT_## AXIS ##_STEP_PIN
       } \
     }while(0)
 
+    #define PULSE_PREP_SHAPING(AXIS, DIVIDEND) do{ \
+      delta_error[_AXIS(AXIS)] += (DIVIDEND); \
+      if ((MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x30000000L) || (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x30000000L)) { \
+        TBI(last_direction_bits, _AXIS(AXIS)); \
+        DIR_WAIT_BEFORE(); \
+        SET_STEP_DIR(AXIS); \
+        DIR_WAIT_AFTER(); \
+      } \
+      step_needed[_AXIS(AXIS)] = (MAXDIR(AXIS) && delta_error[_AXIS(AXIS)] >= 0x10000000L) || \
+                                 (MINDIR(AXIS) && delta_error[_AXIS(AXIS)] <= -0x10000000L); \
+      if (step_needed[_AXIS(AXIS)]) { \
+        count_position[_AXIS(AXIS)] += count_direction[_AXIS(AXIS)]; \
+        delta_error[_AXIS(AXIS)] += MAXDIR(AXIS) ? -0x20000000L : 0x20000000L; \
+      } \
+    }while(0)
+
     // Start an active pulse if needed
     #define PULSE_START(AXIS) do{ \
       if (step_needed[_AXIS(AXIS)]) { \
       } \
     }while(0)
 
-    // Direct Stepping page?
-    const bool is_page = current_block->is_page();
-
     #if ENABLED(DIRECT_STEPPING)
       // Direct stepping is currently not ready for HAS_I_AXIS
       if (is_page) {
     #endif // DIRECT_STEPPING
 
     if (!is_page) {
+      TERN_(INPUT_SHAPING, shaping_queue.enqueue());
+
       // Determine if pulses are needed
       #if HAS_X_STEP
-        PULSE_PREP(X);
+        #if HAS_SHAPING_X
+          PULSE_PREP_SHAPING(X, advance_dividend.x);
+        #else
+          PULSE_PREP(X);
+        #endif
       #endif
       #if HAS_Y_STEP
-        PULSE_PREP(Y);
+        #if HAS_SHAPING_Y
+          PULSE_PREP_SHAPING(Y, advance_dividend.y);
+        #else
+          PULSE_PREP(Y);
+        #endif
       #endif
       #if HAS_Z_STEP
         PULSE_PREP(Z);
 
     // TODO: need to deal with MINIMUM_STEPPER_PULSE over i2s
     #if ISR_MULTI_STEPS
-      START_HIGH_PULSE();
+      START_TIMED_PULSE();
       AWAIT_HIGH_PULSE();
     #endif
 
     #endif
 
     #if ISR_MULTI_STEPS
-      if (events_to_do) START_LOW_PULSE();
+      if (events_to_do) START_TIMED_PULSE();
     #endif
 
   } while (--events_to_do);
 }
 
+#if ENABLED(INPUT_SHAPING)
+
+  void Stepper::shaping_isr() {
+    xyze_bool_t step_needed{0};
+
+    const bool shapex = TERN0(HAS_SHAPING_X, !shaping_queue.peek_x()),
+               shapey = TERN0(HAS_SHAPING_Y, !shaping_queue.peek_y());
+
+    #if HAS_SHAPING_X
+      if (!shaping_dividend_queue.peek_x()) shaping_x.dividend = shaping_dividend_queue.dequeue_x();
+    #endif
+    #if HAS_SHAPING_Y
+      if (!shaping_dividend_queue.peek_y()) shaping_y.dividend = shaping_dividend_queue.dequeue_y();
+    #endif
+
+    #if HAS_SHAPING_X
+      if (shapex) {
+        shaping_queue.dequeue_x();
+        PULSE_PREP_SHAPING(X, shaping_x.dividend);
+        PULSE_START(X);
+      }
+    #endif
+
+    #if HAS_SHAPING_Y
+      if (shapey) {
+        shaping_queue.dequeue_y();
+        PULSE_PREP_SHAPING(Y, shaping_y.dividend);
+        PULSE_START(Y);
+      }
+    #endif
+
+    TERN_(I2S_STEPPER_STREAM, i2s_push_sample());
+
+    if (shapex || shapey) {
+      #if ISR_MULTI_STEPS
+        USING_TIMED_PULSE();
+        START_TIMED_PULSE();
+        AWAIT_HIGH_PULSE();
+      #endif
+      #if HAS_SHAPING_X
+        if (shapex) PULSE_STOP(X);
+      #endif
+      #if HAS_SHAPING_Y
+        if (shapey) PULSE_STOP(Y);
+      #endif
+    }
+  }
+
+#endif // INPUT_SHAPING
+
 // Calculate timer interval, with all limits applied.
 uint32_t Stepper::calc_timer_interval(uint32_t step_rate) {
   #ifdef CPU_32_BIT
       step_event_count = current_block->step_event_count << oversampling;
 
       // Initialize Bresenham delta errors to 1/2
+      #if HAS_SHAPING_X
+        const int32_t old_delta_error_x = delta_error.x;
+      #endif
+      #if HAS_SHAPING_Y
+        const int32_t old_delta_error_y = delta_error.y;
+      #endif
       delta_error = TERN_(LIN_ADVANCE, la_delta_error =) -int32_t(step_event_count);
 
       // Calculate Bresenham dividends and divisors
-      advance_dividend = current_block->steps << 1;
+      advance_dividend = (current_block->steps << 1).asLong();
       advance_divisor = step_event_count << 1;
 
+      // for input shaped axes, advance_divisor is replaced with 0x40000000
+      // and steps are repeated twice so dividends have to be scaled and halved
+      // and the dividend is directional, i.e. signed
+      TERN_(HAS_SHAPING_X, advance_dividend.x = (uint64_t(current_block->steps.x) << 29) / step_event_count);
+      TERN_(HAS_SHAPING_X, if (TEST(current_block->direction_bits, X_AXIS)) advance_dividend.x *= -1);
+      TERN_(HAS_SHAPING_X, if (!shaping_queue.empty_x()) SET_BIT_TO(current_block->direction_bits, X_AXIS, TEST(last_direction_bits, X_AXIS)));
+      TERN_(HAS_SHAPING_Y, advance_dividend.y = (uint64_t(current_block->steps.y) << 29) / step_event_count);
+      TERN_(HAS_SHAPING_Y, if (TEST(current_block->direction_bits, Y_AXIS)) advance_dividend.y *= -1);
+      TERN_(HAS_SHAPING_Y, if (!shaping_queue.empty_y()) SET_BIT_TO(current_block->direction_bits, Y_AXIS, TEST(last_direction_bits, Y_AXIS)));
+
+      // The scaling operation above introduces rounding errors which must now be removed.
+      // For this segment, there will be step_event_count calls to the Bresenham logic and the same number of echoes.
+      // For each pair of calls to the Bresenham logic, delta_error will increase by advance_dividend modulo 0x20000000
+      // so (e.g. for x) delta_error.x will end up changing by (advance_dividend.x * step_event_count) % 0x20000000.
+      // For a divisor which is a power of 2, modulo is the same as as a bitmask, i.e.
+      // (advance_dividend.x * step_event_count) & 0x1FFFFFFF.
+      // This segment's final change in delta_error should actually be zero so we need to increase delta_error by
+      // 0 - ((advance_dividend.x * step_event_count) & 0x1FFFFFFF)
+      // And this needs to be adjusted to the range -0x10000000 to 0x10000000.
+      // Adding and subtracting 0x10000000 inside the outside the modulo achieves this.
+      TERN_(HAS_SHAPING_X, delta_error.x = old_delta_error_x + 0x10000000L - ((0x10000000L + advance_dividend.x * step_event_count) & 0x1FFFFFFFUL));
+      TERN_(HAS_SHAPING_Y, delta_error.y = old_delta_error_y + 0x10000000L - ((0x10000000L + advance_dividend.y * step_event_count) & 0x1FFFFFFFUL));
+
+      // when there is damping, the signal and its echo have different amplitudes
+      #if ENABLED(HAS_SHAPING_X)
+        const int32_t echo_x = shaping_x.factor * (advance_dividend.x >> 7);
+      #endif
+      #if ENABLED(HAS_SHAPING_Y)
+        const int32_t echo_y = shaping_y.factor * (advance_dividend.y >> 7);
+      #endif
+
+      // plan the change of values for advance_dividend for the input shaping echoes
+      TERN_(INPUT_SHAPING, shaping_dividend_queue.enqueue(TERN0(HAS_SHAPING_X, echo_x), TERN0(HAS_SHAPING_Y, echo_y)));
+
+      // apply the adjustment to the primary signal
+      TERN_(HAS_SHAPING_X, advance_dividend.x -= echo_x);
+      TERN_(HAS_SHAPING_Y, advance_dividend.y -= echo_y);
+
       // No step events completed so far
       step_events_completed = 0;
 
       // Enforce a minimum duration for STEP pulse ON
       #if ISR_PULSE_CONTROL
         USING_TIMED_PULSE();
-        START_HIGH_PULSE();
+        START_TIMED_PULSE();
         AWAIT_HIGH_PULSE();
       #endif
 
   #endif
 }
 
+#if ENABLED(INPUT_SHAPING)
+  /**
+   * Calculate a fixed point factor to apply to the signal and its echo
+   * when shaping an axis.
+   */
+  void Stepper::set_shaping_damping_ratio(const AxisEnum axis, const float zeta) {
+    // from the damping ratio, get a factor that can be applied to advance_dividend for fixed point maths
+    // for ZV, we use amplitudes 1/(1+K) and K/(1+K) where K = exp(-zeta * M_PI / sqrt(1.0f - zeta * zeta))
+    // which can be converted to 1:7 fixed point with an excellent fit with a 3rd order polynomial
+    float shaping_factor;
+    if (zeta <= 0.0f) shaping_factor = 64.0f;
+    else if (zeta >= 1.0f) shaping_factor = 0.0f;
+    else {
+      shaping_factor = 64.44056192 + -99.02008832 * zeta;
+      const float zeta2 = zeta * zeta;
+      shaping_factor += -7.58095488 * zeta2;
+      const float zeta3 = zeta2 * zeta;
+      shaping_factor += 43.073216 * zeta3;
+    }
+
+    const bool was_on = hal.isr_state();
+    hal.isr_off();
+    TERN_(HAS_SHAPING_X, if (axis == X_AXIS) { shaping_x.factor = floor(shaping_factor); shaping_x.zeta = zeta; })
+    TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) { shaping_y.factor = floor(shaping_factor); shaping_y.zeta = zeta; })
+    if (was_on) hal.isr_on();
+  }
+
+  float Stepper::get_shaping_damping_ratio(const AxisEnum axis) {
+    TERN_(HAS_SHAPING_X, if (axis == X_AXIS) return shaping_x.zeta);
+    TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.zeta);
+    return -1;
+  }
+
+  void Stepper::set_shaping_frequency(const AxisEnum axis, const float freq) {
+    TERN_(HAS_SHAPING_X, if (axis == X_AXIS) { DelayTimeManager::set_delay(axis, float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq); shaping_x.frequency = freq; })
+    TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) { DelayTimeManager::set_delay(axis, float(uint32_t(STEPPER_TIMER_RATE) / 2) / freq); shaping_y.frequency = freq; })
+  }
+
+  float Stepper::get_shaping_frequency(const AxisEnum axis) {
+    TERN_(HAS_SHAPING_X, if (axis == X_AXIS) return shaping_x.frequency);
+    TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) return shaping_y.frequency);
+    return -1;
+  }
+#endif
+
 /**
  * Set the stepper positions directly in steps
  *
 
   #if EXTRA_CYCLES_BABYSTEP > 20
     #define _SAVE_START() const hal_timer_t pulse_start = HAL_timer_get_count(MF_TIMER_PULSE)
-    #define _PULSE_WAIT() while (EXTRA_CYCLES_BABYSTEP > (uint32_t)(HAL_timer_get_count(MF_TIMER_PULSE) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
+    #define _PULSE_WAIT() while (EXTRA_CYCLES_BABYSTEP > uint32_t(HAL_timer_get_count(MF_TIMER_PULSE) - pulse_start) * (PULSE_TIMER_PRESCALE)) { /* nada */ }
   #else
     #define _SAVE_START() NOOP
     #if EXTRA_CYCLES_BABYSTEP > 0

Marlin/src/module/stepper.h

 
 //static_assert(!any_enable_overlap(), "There is some overlap.");
 
+#if ENABLED(INPUT_SHAPING)
+
+  typedef IF<ENABLED(__AVR__), uint16_t, uint32_t>::type shaping_time_t;
+
+  // These constexpr are used to calculate the shaping queue buffer sizes
+  constexpr xyze_float_t max_feedrate = DEFAULT_MAX_FEEDRATE;
+  constexpr xyze_float_t steps_per_unit = DEFAULT_AXIS_STEPS_PER_UNIT;
+  constexpr float max_steprate = _MAX(LOGICAL_AXIS_LIST(
+                                      max_feedrate.e * steps_per_unit.e,
+                                      max_feedrate.x * steps_per_unit.x,
+                                      max_feedrate.y * steps_per_unit.y,
+                                      max_feedrate.z * steps_per_unit.z,
+                                      max_feedrate.i * steps_per_unit.i,
+                                      max_feedrate.j * steps_per_unit.j,
+                                      max_feedrate.k * steps_per_unit.k,
+                                      max_feedrate.u * steps_per_unit.u,
+                                      max_feedrate.v * steps_per_unit.v,
+                                      max_feedrate.w * steps_per_unit.w
+                                    ));
+  constexpr uint16_t shaping_dividends = max_steprate / _MIN(0x7FFFFFFFL OPTARG(HAS_SHAPING_X, SHAPING_FREQ_X) OPTARG(HAS_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3;
+  constexpr uint16_t shaping_segments = max_steprate / (MIN_STEPS_PER_SEGMENT) / _MIN(0x7FFFFFFFL OPTARG(HAS_SHAPING_X, SHAPING_FREQ_X) OPTARG(HAS_SHAPING_Y, SHAPING_FREQ_Y)) / 2 + 3;
+
+  class DelayTimeManager {
+    private:
+      static shaping_time_t now;
+      #ifdef HAS_SHAPING_X
+        static shaping_time_t delay_x;
+      #endif
+      #ifdef HAS_SHAPING_Y
+        static shaping_time_t delay_y;
+      #endif
+    public:
+      static void decrement_delays(const shaping_time_t interval) { now += interval; }
+      static void set_delay(const AxisEnum axis, const shaping_time_t delay) {
+        TERN_(HAS_SHAPING_X, if (axis == X_AXIS) delay_x = delay);
+        TERN_(HAS_SHAPING_Y, if (axis == Y_AXIS) delay_y = delay);
+      }
+  };
+
+  template<int SIZE>
+  class DelayQueue : public DelayTimeManager {
+    protected:
+      shaping_time_t times[SIZE];
+      uint16_t tail = 0 OPTARG(HAS_SHAPING_X, head_x = 0) OPTARG(HAS_SHAPING_Y, head_y = 0);
+
+    public:
+      void enqueue() {
+        times[tail] = now;
+        if (++tail == SIZE) tail = 0;
+      }
+      #ifdef HAS_SHAPING_X
+        shaping_time_t peek_x() {
+          if (head_x != tail) return times[head_x] + delay_x - now;
+          else return shaping_time_t(-1);
+        }
+        void dequeue_x() { if (++head_x == SIZE) head_x = 0; }
+        bool empty_x() { return head_x == tail; }
+        uint16_t free_count_x() { return head_x > tail ? head_x - tail - 1 : head_x + SIZE - tail - 1; }
+      #endif
+      #ifdef HAS_SHAPING_Y
+        shaping_time_t peek_y() {
+          if (head_y != tail) return times[head_y] + delay_y - now;
+          else return shaping_time_t(-1);
+        }
+        void dequeue_y() { if (++head_y == SIZE) head_y = 0; }
+        bool empty_y() { return head_y == tail; }
+        uint16_t free_count_y() { return head_y > tail ? head_y - tail - 1 : head_y + SIZE - tail - 1; }
+      #endif
+      void purge() { auto temp = TERN_(HAS_SHAPING_X, head_x) = TERN_(HAS_SHAPING_Y, head_y) = tail; UNUSED(temp);}
+  };
+
+  class ParamDelayQueue : public DelayQueue<shaping_segments> {
+    private:
+      #ifdef HAS_SHAPING_X
+        int32_t params_x[shaping_segments];
+      #endif
+      #ifdef HAS_SHAPING_Y
+        int32_t params_y[shaping_segments];
+      #endif
+
+    public:
+      void enqueue(const int32_t param_x, const int32_t param_y) {
+        TERN(HAS_SHAPING_X, params_x[DelayQueue<shaping_segments>::tail] = param_x, UNUSED(param_x));
+        TERN(HAS_SHAPING_Y, params_y[DelayQueue<shaping_segments>::tail] = param_y, UNUSED(param_y));
+        DelayQueue<shaping_segments>::enqueue();
+      }
+      #ifdef HAS_SHAPING_X
+        const int32_t dequeue_x() {
+          const int32_t result = params_x[DelayQueue<shaping_segments>::head_x];
+          DelayQueue<shaping_segments>::dequeue_x();
+          return result;
+        }
+      #endif
+      #ifdef HAS_SHAPING_Y
+        const int32_t dequeue_y() {
+          const int32_t result = params_y[DelayQueue<shaping_segments>::head_y];
+          DelayQueue<shaping_segments>::dequeue_y();
+          return result;
+        }
+      #endif
+  };
+
+  struct ShapeParams {
+    float frequency;
+    float zeta;
+    uint8_t factor;
+    int32_t dividend;
+  };
+
+#endif // INPUT_SHAPING
+
 //
 // Stepper class definition
 //
 
     // Delta error variables for the Bresenham line tracer
     static xyze_long_t delta_error;
-    static xyze_ulong_t advance_dividend;
+    static xyze_long_t advance_dividend;
     static uint32_t advance_divisor,
                     step_events_completed,  // The number of step events executed in the current block
                     accelerate_until,       // The point from where we need to stop acceleration
       static bool bezier_2nd_half; // If Bézier curve has been initialized or not
     #endif
 
+    #if ENABLED(INPUT_SHAPING)
+      static ParamDelayQueue shaping_dividend_queue;
+      static DelayQueue<shaping_dividends> shaping_queue;
+      #if HAS_SHAPING_X
+        static ShapeParams shaping_x;
+      #endif
+      #if HAS_SHAPING_Y
+        static ShapeParams shaping_y;
+      #endif
+    #endif
+
     #if ENABLED(LIN_ADVANCE)
       static constexpr uint32_t LA_ADV_NEVER = 0xFFFFFFFF;
       static uint32_t nextAdvanceISR,
     // The stepper block processing ISR phase
     static uint32_t block_phase_isr();
 
+    #if ENABLED(INPUT_SHAPING)
+      static void shaping_isr();
+    #endif
+
     #if ENABLED(LIN_ADVANCE)
       // The Linear advance ISR phase
       static void advance_isr();
       set_directions();
     }
 
+    #if ENABLED(INPUT_SHAPING)
+      static void set_shaping_damping_ratio(const AxisEnum axis, const float zeta);
+      static float get_shaping_damping_ratio(const AxisEnum axis);
+      static void set_shaping_frequency(const AxisEnum axis, const float freq);
+      static float get_shaping_frequency(const AxisEnum axis);
+    #endif
+
   private:
 
     // Set the current position in steps

buildroot/tests/mega2560

         FIL_RUNOUT2_PIN 16 FIL_RUNOUT3_PIN 17 FIL_RUNOUT4_PIN 4 FIL_RUNOUT5_PIN 5
 opt_enable MIXING_EXTRUDER GRADIENT_MIX GRADIENT_VTOOL CR10_STOCKDISPLAY \
            USE_CONTROLLER_FAN CONTROLLER_FAN_EDITABLE CONTROLLER_FAN_IGNORE_Z \
-           FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE
+           FILAMENT_RUNOUT_SENSOR ADVANCED_PAUSE_FEATURE NOZZLE_PARK_FEATURE INPUT_SHAPING
 opt_disable DISABLE_INACTIVE_EXTRUDER
 exec_test $1 $2 "Azteeg X3 | Mixing Extruder (x5) | Gradient Mix | Greek" "$3"
 

ini/features.ini

 LIN_ADVANCE                            = src_filter=+<src/gcode/feature/advance>
 PHOTO_GCODE                            = src_filter=+<src/gcode/feature/camera>
 CONTROLLER_FAN_EDITABLE                = src_filter=+<src/gcode/feature/controllerfan>
+INPUT_SHAPING                          = src_filter=+<src/gcode/feature/input_shaping>
 GCODE_MACROS                           = src_filter=+<src/gcode/feature/macro>
 GRADIENT_MIX                           = src_filter=+<src/gcode/feature/mixing/M166.cpp>
 HAS_SAVED_POSITIONS                    = src_filter=+<src/gcode/feature/pause/G60.cpp> +<src/gcode/feature/pause/G61.cpp>

platformio.ini

   -<src/gcode/feature/advance>
   -<src/gcode/feature/camera>
   -<src/gcode/feature/i2c>
+  -<src/gcode/feature/input_shaping>
   -<src/gcode/feature/L6470>
   -<src/gcode/feature/leds/M150.cpp>
   -<src/gcode/feature/leds/M7219.cpp>