diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 830db40333..de1c841f1c 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1177,6 +1177,7 @@ #define FTM_SHAPING_ZETA_E 0.03f // Zeta used by input shapers for E axis #define FTM_SHAPING_V_TOL_E 0.05f // Vibration tolerance used by EI input shapers for E axis + //#define FTM_RESONANCE_TEST // Sine sweep motion for resonance study //#define FTM_SMOOTHING // Smoothing can reduce artifacts and make steppers quieter // on sharp corners, but too much will round corners. #if ENABLED(FTM_SMOOTHING) diff --git a/Marlin/src/core/types.h b/Marlin/src/core/types.h index 23d83c4b9e..486f78fde8 100644 --- a/Marlin/src/core/types.h +++ b/Marlin/src/core/types.h @@ -867,7 +867,7 @@ struct XYZEval { FI void set(const XYZval &pxyz, const T pe) { set(pxyz); e = pe; } FI void set(LOGICAL_AXIS_ARGS_LC(const T)) { LOGICAL_AXIS_CODE(_e = e, a = x, b = y, c = z, _i = i, _j = j, _k = k, _u = u, _v = v, _w = w); } #if DISTINCT_AXES > LOGICAL_AXES - FI void set(const T (&arr)[DISTINCT_AXES]) { LOGICAL_AXIS_CODE(e = arr[LOGICAL_AXES-1], x = arr[0], y = arr[1], z = arr[2], i = arr[3], j = arr[4], k = arr[5], u = arr[6], v = arr[7], w = arr[8]); } + FI void set(const T (&arr)[DISTINCT_AXES], const uint8_t eindex) { LOGICAL_AXIS_CODE(e = arr[LOGICAL_AXES-1 + eindex], x = arr[0], y = arr[1], z = arr[2], i = arr[3], j = arr[4], k = arr[5], u = arr[6], v = arr[7], w = arr[8]); } #endif #endif diff --git a/Marlin/src/feature/e_parser.cpp b/Marlin/src/feature/e_parser.cpp index e249d81969..9e974f2e83 100644 --- a/Marlin/src/feature/e_parser.cpp +++ b/Marlin/src/feature/e_parser.cpp @@ -33,6 +33,9 @@ // Static data members bool EmergencyParser::killed_by_M112, // = false EmergencyParser::quickstop_by_M410, + #if ENABLED(FTM_RESONANCE_TEST) + EmergencyParser::rt_stop_by_M496, // = false + #endif #if HAS_MEDIA EmergencyParser::sd_abort_by_M524, #endif @@ -147,9 +150,22 @@ void EmergencyParser::update(EmergencyParser::State &state, const uint8_t c) { case EP_M10: state = (c == '8') ? EP_M108 : EP_IGNORE; break; case EP_M11: state = (c == '2') ? EP_M112 : EP_IGNORE; break; - case EP_M4: state = (c == '1') ? EP_M41 : EP_IGNORE; break; + case EP_M4: + switch (c) { + case '1' :state = EP_M41; break; + #if ENABLED(FT_MOTION_RESONANCE_TEST) + case '9': state = EP_M49; break; + #endif + default: state = EP_IGNORE; + } + break; + case EP_M41: state = (c == '0') ? EP_M410 : EP_IGNORE; break; + #if ENABLED(FTM_RESONANCE_TEST) + case EP_M49: state = (c == '6') ? EP_M496 : EP_IGNORE; break; + #endif + #if HAS_MEDIA case EP_M5: state = (c == '2') ? EP_M52 : EP_IGNORE; break; case EP_M52: state = (c == '4') ? EP_M524 : EP_IGNORE; break; @@ -195,6 +211,9 @@ void EmergencyParser::update(EmergencyParser::State &state, const uint8_t c) { case EP_M108: wait_for_user = wait_for_heatup = false; break; case EP_M112: killed_by_M112 = true; break; case EP_M410: quickstop_by_M410 = true; break; + #if ENABLED(FTM_RESONANCE_TEST) + case EP_M496: rt_stop_by_M496 = true; break; + #endif #if ENABLED(EP_BABYSTEPPING) case EP_M293: babystep.ep_babysteps++; break; case EP_M294: babystep.ep_babysteps--; break; diff --git a/Marlin/src/feature/e_parser.h b/Marlin/src/feature/e_parser.h index 8dacb0581c..17e85a331d 100644 --- a/Marlin/src/feature/e_parser.h +++ b/Marlin/src/feature/e_parser.h @@ -43,6 +43,9 @@ public: #if HAS_MEDIA EP_M5, EP_M52, EP_M524, #endif + #if ENABLED(FTM_RESONANCE_TEST) + EP_M49, EP_M496, + #endif #if ENABLED(EP_BABYSTEPPING) EP_M2, EP_M29, EP_M293, EP_M294, #endif @@ -64,6 +67,10 @@ public: static bool killed_by_M112; static bool quickstop_by_M410; + #if ENABLED(FTM_RESONANCE_TEST) + static bool rt_stop_by_M496; + #endif + #if HAS_MEDIA static bool sd_abort_by_M524; #endif diff --git a/Marlin/src/gcode/feature/ft_motion/M495_M496.cpp b/Marlin/src/gcode/feature/ft_motion/M495_M496.cpp new file mode 100644 index 0000000000..15e1a1fe0f --- /dev/null +++ b/Marlin/src/gcode/feature/ft_motion/M495_M496.cpp @@ -0,0 +1,186 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2025 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 . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(FTM_RESONANCE_TEST) + +#include "../../gcode.h" +#include "../../../module/ft_motion.h" +#include "../../../module/ft_motion/resonance_generator.h" + +void say_resonance_test() { + const ftm_resonance_test_params_t &p = ftMotion.rtg.rt_params; + SERIAL_ECHO_START(); + SERIAL_ECHOLN(F("M495 "), F("Resonance Test")); + SERIAL_ECHOLNPGM(" Axis: ", p.axis == NO_AXIS_ENUM ? C('-') : C(AXIS_CHAR(p.axis))); + SERIAL_ECHOLNPGM(" Freq Range (F..T): ", p.min_freq, " .. ", p.max_freq, " Hz"); + SERIAL_ECHOLNPGM(" Octave Duration (O): ", p.octave_duration, " s"); + SERIAL_ECHOLNPGM(" Accel/Hz (A): ", p.accel_per_hz); +} + +/** + * M495: Configure and run the resonance test. + * With no parameters report the current settings. + * + * Parameters: + * A Accel per Hz. (Default 60) + * F Start frequency. (Default 5.0) + * S Start the test. + * T End frequency. (Default 100.0f) + * O Octave duration for logarithmic progression + * C Amplitude correction factor. (Default 5) + * X Flag to select the X axis. + * Y Flag to select the Y axis. + * Z Flag to select the Z axis. + * H Get the Resonance Frequency from Timeline value. (Default 0) + * + * Examples: + * M495 S : Start the test with default or last-used parameters + * M495 X S : Start the test on the X axis with default or last-used parameters + * M495 H : Get Resonance Frequency from Timeline value + * + */ +void GcodeSuite::M495() { + if (!parser.seen_any()) return say_resonance_test(); + + ftm_resonance_test_params_t &p = ftMotion.rtg.rt_params; + + const bool seenX = parser.seen_test('X'), seenY = parser.seen_test('Y'), seenZ = parser.seen_test('Z'); + + if (seenX + seenY + seenZ == 1) { + const AxisEnum a = seenX ? X_AXIS : seenY ? Y_AXIS : Z_AXIS; + p.axis = a; + SERIAL_ECHOLN(C(AXIS_CHAR(a)), F("-axis selected"), F(" for "), F("Resonance Test")); + } + else if (seenX + seenY + seenZ > 1) { + SERIAL_ECHOLN(F("?Specify X, Y, or Z axis"), F(" for "), F("Resonance Test")); + return; + } + + if (parser.seenval('A')) { + const float val = parser.value_float(); + if (p.axis == Z_AXIS && val > 15.0f) { + p.accel_per_hz = 15.0f; + SERIAL_ECHOLNPGM("Accel/Hz set to max 15 mm/s for Z Axis"); + } + else { + p.accel_per_hz = val; + SERIAL_ECHOLNPGM("Accel/Hz set to ", p.accel_per_hz); + } + } + + if (parser.seenval('F')) { + const float val = parser.value_float(); + if (val >= 5.0f) { + p.min_freq = val; + SERIAL_ECHOLNPGM("Start Frequency set to ", p.min_freq, " Hz"); + } + else { + SERIAL_ECHOLN(F("?Invalid "), F("Start [F]requency. (minimum 5.0 Hz)")); + } + } + + if (parser.seenval('T')) { + const float val = parser.value_float(); + if (val > p.min_freq && val <= 200.0f) { + p.max_freq = val; + SERIAL_ECHOLNPGM("End Frequency set to ", p.max_freq, " Hz"); + } + else { + SERIAL_ECHOLN(F("?Invalid "), F("End Frequency [T]. (StartFreq .. 200 Hz)")); + } + } + + if (parser.seenval('O')) { + const float val = parser.value_float(); + if (WITHIN(val, 20, 60)) { + p.octave_duration = val; + SERIAL_ECHOLNPGM("Octave Duration set to ",p.octave_duration, " s"); + } + else { + SERIAL_ECHOLN(F("?Invalid "), F("octave duration [O]. (20..60 s)")); + } + } + + if (parser.seenval('C')) { + const int val = parser.value_int(); + if (WITHIN(val, 1, 8)) { + p.amplitude_correction = val; + SERIAL_ECHOLNPGM("Amplitude Correction Factor set to ", p.amplitude_correction); + } + else { + SERIAL_ECHOLN(F("?Invalid "), F("Amplitude [C]orrection Factor. (1..8)")); + } + } + + if (parser.seenval('G')) { + const float val = parser.value_float(); + if (WITHIN(val, 0, 100)) { + ftMotion.rtg.timeline = val; + SERIAL_ECHOLNPGM("Resonance Frequency set to ", ftMotion.rtg.getFrequencyFromTimeline(), " Hz"); + } + else { + SERIAL_ECHOLN(F("?Invalid "), F("Timeline value (0..100 s)")); + } + } + + if (parser.seen_test('S')) { + if (ftMotion.cfg.active) { + if (p.axis != NO_AXIS_ENUM) { + if (p.max_freq > p.min_freq) { + SERIAL_ECHOLN(F("Starting "), F("Resonance Test")); + ftMotion.start_resonance_test(); + // The function returns immediately, the test runs in the background. + } + else { + SERIAL_ECHOLNPGM("?End Frequency must be greater than Start Frequency"); + } + } + else { + SERIAL_ECHOLN(F("?Specify X, Y, or Z axis"), F(" first")); + } + } + else { + SERIAL_ECHOLN(F("?Activate FT Motion to run the "), F("Resonance Test")); + } + } +} + +/** + * M496: Abort the resonance test (via Emergency Parser) + */ +void GcodeSuite::M496() { + if (ftMotion.rtg.isActive()) { + ftMotion.rtg.abort(); + EmergencyParser::rt_stop_by_M496 = false; + #if DISABLED(MARLIN_SMALL_BUILD) + SERIAL_ECHOLN(F("Resonance Test"), F(" aborted.")); + #endif + return; + } + #if DISABLED(MARLIN_SMALL_BUILD) + SERIAL_ECHOLN(F("No active "), F("Resonance Test"), F(" to abort.")); + #endif +} + +#endif // FTM_RESONANCE_TEST diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp index 20b268256e..f7ecbbd1d3 100644 --- a/Marlin/src/gcode/gcode.cpp +++ b/Marlin/src/gcode/gcode.cpp @@ -926,6 +926,10 @@ void GcodeSuite::process_parsed_command(bool no_ok/*=false*/) { #if ENABLED(FTM_SMOOTHING) case 494: M494(); break; // M494: Fixed-Time Motion extras #endif + #if ENABLED(FTM_RESONANCE_TEST) + case 495: M495(); break; // M495: Resonance test for Input Shaping + case 496: M496(); break; // M496: Abort resonance test + #endif #endif case 500: M500(); break; // M500: Store settings in EEPROM diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h index a08ae769fe..e288b9bd99 100644 --- a/Marlin/src/gcode/gcode.h +++ b/Marlin/src/gcode/gcode.h @@ -252,6 +252,8 @@ * M485 - Send RS485 packets (Requires RS485_SERIAL_PORT) * M486 - Identify and cancel objects. (Requires CANCEL_OBJECTS) * M493 - Set / Report input FT Motion/Shaping parameters. (Requires FT_MOTION) + * M495 - Set / Start resonance test. (Requires FTM_RESONANCE_TEST) + * M496 - Abort resonance test. (Requires FTM_RESONANCE_TEST) * M500 - Store parameters in EEPROM. (Requires EEPROM_SETTINGS) * M501 - Restore parameters from EEPROM. (Requires EEPROM_SETTINGS) * M502 - Revert to the default "factory settings". ** Does not write them to EEPROM! ** @@ -1115,6 +1117,11 @@ private: static void M493_report(const bool forReplay=true); static void M494(); static void M494_report(const bool forReplay=true); + #if ENABLED(FTM_RESONANCE_TEST) + static void M495(); + static void M495_report(const bool forReplay=true); + static void M496(); + #endif #endif static void M500(); diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index cd77bd24ba..4bc63660a2 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4510,6 +4510,9 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." static_assert(FTM_SMOOTHING_TIME_Z <= FTM_MAX_SMOOTHING_TIME, "FTM_SMOOTHING_TIME_Z must be <= FTM_MAX_SMOOTHING_TIME."); static_assert(FTM_SMOOTHING_TIME_E <= FTM_MAX_SMOOTHING_TIME, "FTM_SMOOTHING_TIME_E must be <= FTM_MAX_SMOOTHING_TIME."); #endif + #if ENABLED(FTM_RESONANCE_TEST) && DISABLED(EMERGENCY_PARSER) + #error "EMERGENCY_PARSER is required with FTM_RESONANCE_TEST (to cancel the test)." + #endif #endif // Multi-Stepping Limit diff --git a/Marlin/src/lcd/language/language_en.h b/Marlin/src/lcd/language/language_en.h index 05c5fd43dd..a1ba59bcca 100644 --- a/Marlin/src/lcd/language/language_en.h +++ b/Marlin/src/lcd/language/language_en.h @@ -951,6 +951,14 @@ namespace LanguageNarrow_en { LSTR MSG_FTM_SMOOTH_TIME_N = _UxGT("@ Smoothing Time"); LSTR MSG_FTM_POLY6_OVERSHOOT = _UxGT("@ Poly6 Overshoot"); + LSTR MSG_FTM_RESONANCE_TEST = _UxGT("Resonance Test"); + LSTR MSG_FTM_RT_RUNNING = _UxGT("Res. Test Running..."); + LSTR MSG_FTM_RT_START_N = _UxGT("Start @ Axis Test"); + LSTR MSG_FTM_RT_STOP = _UxGT("Abort Test"); + LSTR MSG_FTM_RETRIEVE_FREQ = _UxGT("Calc. Res. Freq."); + LSTR MSG_FTM_RESONANCE_FREQ = _UxGT("Resonance Freq."); + LSTR MSG_FTM_TIMELINE_FREQ = _UxGT("Timeline (s)"); + LSTR MSG_LEVEL_X_AXIS = _UxGT("Level X Axis"); LSTR MSG_AUTO_CALIBRATE = _UxGT("Auto Calibrate"); LSTR MSG_FTDI_HEATER_TIMEOUT = _UxGT("Idle timeout, temperature decreased. Press Okay to reheat and again to resume."); @@ -1158,6 +1166,8 @@ namespace LanguageWide_en { LSTR MSG_EEPROM_INITIALIZED = _UxGT("Default Settings Restored"); LSTR MSG_PREHEAT_M_CHAMBER = _UxGT("Preheat $ Chamber"); LSTR MSG_PREHEAT_M_SETTINGS = _UxGT("Preheat $ Config"); + LSTR MSG_FTM_RT_RUNNING = _UxGT("Resonance Test Running..."); + LSTR MSG_FTM_RESONANCE_FREQ = _UxGT("Resonance frequency"); #endif } diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index 0d23484f3f..dd75cc5c29 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -382,6 +382,38 @@ void menu_move() { END_MENU(); } + #if ENABLED(FTM_RESONANCE_TEST) + + void menu_ftm_resonance_freq() { + START_MENU(); + BACK_ITEM(MSG_FTM_RESONANCE_TEST); + + STATIC_ITEM(MSG_FTM_RETRIEVE_FREQ); + EDIT_ITEM(float62, MSG_FTM_TIMELINE_FREQ, &ftMotion.rtg.timeline, 0.0f, 600.0f); + PSTRING_ITEM(MSG_FTM_RESONANCE_FREQ, ftostr53_63(ftMotion.rtg.getFrequencyFromTimeline()), SS_FULL); + + END_MENU(); + } + + void menu_ftm_resonance_test() { + START_MENU(); + BACK_ITEM(MSG_FIXED_TIME_MOTION); + + if (ftMotion.rtg.isActive() && !ftMotion.rtg.isDone()) { + STATIC_ITEM(MSG_FTM_RT_RUNNING); + ACTION_ITEM(MSG_FTM_RT_STOP, []{ ftMotion.rtg.abort(); ui.go_back(); }); + } + else { + GCODES_ITEM_N(X_AXIS, MSG_FTM_RT_START_N, F("M495 X S")); + GCODES_ITEM_N(Y_AXIS, MSG_FTM_RT_START_N, F("M495 Y S")); + GCODES_ITEM_N(Z_AXIS, MSG_FTM_RT_START_N, F("M495 Z S")); + SUBMENU(MSG_FTM_RETRIEVE_FREQ, menu_ftm_resonance_freq); + } + END_MENU(); + } + + #endif // FTM_RESONANCE_TEST + #if HAS_DYNAMIC_FREQ void menu_ftm_dyn_mode() { @@ -504,6 +536,10 @@ void menu_move() { #if ENABLED(FTM_SMOOTHING) CARTES_MAP(_SMOO_MENU_ITEM); #endif + + #if ENABLED(FTM_RESONANCE_TEST) + SUBMENU(MSG_FTM_RESONANCE_TEST, menu_ftm_resonance_test); + #endif } END_MENU(); } // menu_ft_motion diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index 0a2c9f6c3a..74ef6e79ef 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -35,6 +35,11 @@ #include "ft_motion/trajectory_trapezoidal.h" #include "ft_motion/trajectory_poly5.h" #include "ft_motion/trajectory_poly6.h" +#if ENABLED(FTM_RESONANCE_TEST) + #include "ft_motion/resonance_generator.h" + #include "../gcode/gcode.h" // for home_all_axes +#endif + #include "stepper.h" // Access stepper block queue function and abort status. #include "endstops.h" @@ -71,6 +76,9 @@ Poly6TrajectoryGenerator FTMotion::poly6Generator; TrajectoryGenerator* FTMotion::currentGenerator = &FTMotion::trapezoidalGenerator; TrajectoryType FTMotion::trajectoryType = TrajectoryType::FTM_TRAJECTORY_TYPE; +// Resonance Test +TERN_(FTM_RESONANCE_TEST,ResonanceGenerator FTMotion::rtg;) // Resonance trajectory generator instance + // Compact plan buffer stepper_plan_t FTMotion::stepper_plan_buff[FTM_BUFFER_SIZE]; XYZEval FTMotion::curr_steps_q32_32 = {0}; @@ -150,14 +158,32 @@ void FTMotion::loop() { * 3. Reset all the states / memory. * 4. Signal ready for new block. */ - if (stepper.abort_current_block) { - discard_planner_block_protected(); - reset(); - currentGenerator->planRunout(0.0f); // Reset generator state - stepper.abort_current_block = false; // Abort finished. - } - fill_stepper_plan_buffer(); + const bool using_resonance = TERN(FTM_RESONANCE_TEST, rtg.isActive(), false); + + #if ENABLED(FTM_RESONANCE_TEST) + if (using_resonance) { + // Resonance Test has priority over normal ft_motion operation. + // Process resonance test if active. When it's done, generate the last data points for a clean ending. + if (rtg.isActive()) { + if (rtg.isDone()) { + rtg.abort(); + return; + } + rtg.fill_stepper_plan_buffer(); + } + } + #endif + + if (!using_resonance) { + if (stepper.abort_current_block) { + discard_planner_block_protected(); + reset(); + currentGenerator->planRunout(0.0f); // Reset generator state + stepper.abort_current_block = false; // Abort finished. + } + fill_stepper_plan_buffer(); + } // Set busy status for use by planner.busy() const bool oldBusy = busy; @@ -557,4 +583,25 @@ void FTMotion::fill_stepper_plan_buffer() { } } +#if ENABLED(FTM_RESONANCE_TEST) + + // Start Resonance Testing + void FTMotion::start_resonance_test() { + home_if_needed(); // Ensure known axes first + + ftm_resonance_test_params_t &p = rtg.rt_params; + + // Safe Acceleration per Hz for Z axis + if (p.axis == Z_AXIS && p.accel_per_hz > 15.0f) + p.accel_per_hz = 15.0f; + + // Always move to the center of the bed + do_blocking_move_to_xy(X_CENTER, Y_CENTER, Z_CLEARANCE_FOR_HOMING); + + // Start test at the current position with the configured time-step + rtg.start(current_position, FTM_TS); + } + +#endif // FTM_RESONANCE_TEST + #endif // FT_MOTION diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index bec65f2abf..6e5b36cbe4 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -28,6 +28,9 @@ #include "ft_motion/trajectory_trapezoidal.h" #include "ft_motion/trajectory_poly5.h" #include "ft_motion/trajectory_poly6.h" +#if ENABLED(FTM_RESONANCE_TEST) + #include "ft_motion/resonance_generator.h" +#endif #if HAS_FTM_SHAPING #include "ft_motion/shaping.h" @@ -88,6 +91,10 @@ typedef struct FTConfig { */ class FTMotion { + #if ENABLED(FTM_RESONANCE_TEST) + friend void ResonanceGenerator::fill_stepper_plan_buffer(); + #endif + public: // Public variables @@ -140,6 +147,10 @@ class FTMotion { // Public methods static void init(); static void loop(); // Controller main, to be invoked from non-isr task. + #if ENABLED(FTM_RESONANCE_TEST) + static void start_resonance_test(); // Start a resonance test with given parameters + static ResonanceGenerator rtg; // Resonance trajectory generator instance + #endif #if HAS_FTM_SHAPING // Refresh gains and indices used by shaping functions. diff --git a/Marlin/src/module/ft_motion/resonance_generator.cpp b/Marlin/src/module/ft_motion/resonance_generator.cpp new file mode 100644 index 0000000000..f1b14ab72a --- /dev/null +++ b/Marlin/src/module/ft_motion/resonance_generator.cpp @@ -0,0 +1,88 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2025 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 . + * + */ + +#include "../../inc/MarlinConfigPre.h" + +#if ENABLED(FTM_RESONANCE_TEST) + +#include "../ft_motion.h" +#include "resonance_generator.h" + +#include + +ftm_resonance_test_params_t ResonanceGenerator::rt_params; // Resonance test parameters + +bool ResonanceGenerator::active = false; // Resonance test active +bool ResonanceGenerator::done = false; // Resonance test done +float ResonanceGenerator::rt_time = FTM_TS; // Resonance test timer +float ResonanceGenerator::timeline = 0.0f; + +ResonanceGenerator::ResonanceGenerator() {} + +void ResonanceGenerator::abort() { + reset(); + ftMotion.reset(); +} + +void ResonanceGenerator::reset() { + rt_params = ftm_resonance_test_params_t(); + rt_time = FTM_TS; + active = false; + done = false; +} + +void ResonanceGenerator::fill_stepper_plan_buffer() { + xyze_float_t traj_coords = {}; + + while (!ftMotion.stepper_plan_is_full()) { + // Calculate current frequency + // Logarithmic approach with duration per octave + const float freq = rt_params.min_freq * powf(2.0f, rt_time / rt_params.octave_duration); + if (freq > rt_params.max_freq) { + done = true; + return; + } + + // Amplitude based on a sinusoidal wave : A = accel / (4 * PI^2 * f^2) + //const float accel_magnitude = rt_params.accel_per_hz * freq; + //const float amplitude = rt_params.amplitude_correction * accel_magnitude / (4.0f * sq(M_PI) * sq(freq)); + const float amplitude = rt_params.amplitude_correction * rt_params.accel_per_hz * 0.25f / (sq(M_PI) * freq); + + // Phase in radians + const float phase = 2.0f * M_PI * freq * rt_time; + + // Position Offset : between -A and +A + const float pos_offset = amplitude * sinf(phase); + + // Set base position and apply offset to the test axis in one step for all axes + #define _SET_TRAJ(A) traj_coords.A = rt_params.start_pos.A + (rt_params.axis == A##_AXIS ? pos_offset : 0.0f); + LOGICAL_AXIS_MAP(_SET_TRAJ); + + stepper_plan_t plan = ftMotion.calc_stepper_plan(traj_coords); + // Store in buffer + ftMotion.enqueue_stepper_plan(plan); + // Increment time for the next point + rt_time += FTM_TS; + } +} + +#endif // FTM_RESONANCE_TEST diff --git a/Marlin/src/module/ft_motion/resonance_generator.h b/Marlin/src/module/ft_motion/resonance_generator.h new file mode 100644 index 0000000000..16156693dd --- /dev/null +++ b/Marlin/src/module/ft_motion/resonance_generator.h @@ -0,0 +1,73 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2025 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 . + * + */ +#pragma once + +#include "../../inc/MarlinConfigPre.h" + +#include + +typedef struct FTMResonanceTestParams { + AxisEnum axis = NO_AXIS_ENUM; // Axis to test + float min_freq = 5.0f; // Minimum frequency [Hz] + float max_freq = 100.0f; // Maximum frequency [Hz] + float octave_duration = 40.0f; // Octave duration for logarithmic progression + float accel_per_hz = 60.0f; // Acceleration per Hz [mm/sec/Hz] or [g/Hz] + int16_t amplitude_correction = 5; // Amplitude correction factor + xyze_pos_t start_pos; // Initial stepper position +} ftm_resonance_test_params_t; + +class ResonanceGenerator { + public: + static ftm_resonance_test_params_t rt_params; // Resonance test parameters + static float timeline; // Timeline Value to calculate resonance frequency + + ResonanceGenerator(); + + void planRunout(const float duration); + + void reset(); + + void start(const xyze_pos_t &spos, const float t) { + rt_params.start_pos = spos; + rt_time = t; + active = true; + done = false; + } + + float getFrequencyFromTimeline() { + return (rt_params.min_freq * powf(2.0f, timeline / rt_params.octave_duration)); // Return frequency based on timeline + } + + void fill_stepper_plan_buffer(); // Fill stepper plan buffer with trajectory points + + bool isActive() const { return active; } + bool isDone() const { return done; } + void setActive(bool state) { active = state; } + void setDone(bool state) { done = state; } + + void abort(); // Abort resonance test + + private: + static float rt_time; // Test timer + static bool active; // Resonance test active + static bool done; // Resonance test done +}; diff --git a/Marlin/src/module/ft_motion/stepping.h b/Marlin/src/module/ft_motion/stepping.h index 6d0f3d31a5..2b9ad3a21a 100644 --- a/Marlin/src/module/ft_motion/stepping.h +++ b/Marlin/src/module/ft_motion/stepping.h @@ -29,7 +29,6 @@ typedef struct stepper_plan { void reset() { advance_dividend_q0_32.reset(); } } stepper_plan_t; - // Stepping plan handles steps for a while frame (trajectory point delta) typedef struct Stepping { stepper_plan_t stepper_plan; diff --git a/Marlin/src/module/ft_motion/trajectory_generator.h b/Marlin/src/module/ft_motion/trajectory_generator.h index 934394940a..677249af0d 100644 --- a/Marlin/src/module/ft_motion/trajectory_generator.h +++ b/Marlin/src/module/ft_motion/trajectory_generator.h @@ -21,7 +21,7 @@ */ #pragma once -#include +#include "../../inc/MarlinConfigPre.h" /** * Base class for trajectory generators. @@ -72,4 +72,7 @@ protected: /** * Trajectory generator types for runtime selection */ -enum class TrajectoryType : uint8_t { TRAPEZOIDAL, POLY5, POLY6 }; +enum class TrajectoryType : uint8_t { + TRAPEZOIDAL, POLY5, POLY6 + OPTARG(FTM_RESONANCE_TEST, RESONANCE) +}; diff --git a/Marlin/src/module/ft_motion/trajectory_poly6.cpp b/Marlin/src/module/ft_motion/trajectory_poly6.cpp index a16f344217..338c7b0d5d 100644 --- a/Marlin/src/module/ft_motion/trajectory_poly6.cpp +++ b/Marlin/src/module/ft_motion/trajectory_poly6.cpp @@ -25,8 +25,8 @@ #if ENABLED(FT_MOTION) #include "trajectory_poly6.h" -#include #include "../ft_motion.h" +#include Poly6TrajectoryGenerator::Poly6TrajectoryGenerator() {} diff --git a/buildroot/tests/STM32F103RC_btt b/buildroot/tests/STM32F103RC_btt index 97339d7750..369fa80978 100755 --- a/buildroot/tests/STM32F103RC_btt +++ b/buildroot/tests/STM32F103RC_btt @@ -14,6 +14,7 @@ opt_set MOTHERBOARD BOARD_BTT_SKR_MINI_E3_V1_0 SERIAL_PORT 1 SERIAL_PORT_2 -1 \ X_DRIVER_TYPE TMC2209 Y_DRIVER_TYPE TMC2209 Z_DRIVER_TYPE TMC2209 E0_DRIVER_TYPE TMC2209 \ X_CURRENT_HOME X_CURRENT/2 Y_CURRENT_HOME Y_CURRENT/2 Z_CURRENT_HOME Y_CURRENT/2 opt_enable CR10_STOCKDISPLAY PINS_DEBUGGING Z_IDLE_HEIGHT EDITABLE_HOMING_CURRENT \ - FT_MOTION FT_MOTION_MENU BIQU_MICROPROBE_V1 PROBE_ENABLE_DISABLE Z_SAFE_HOMING AUTO_BED_LEVELING_BILINEAR \ - ADAPTIVE_STEP_SMOOTHING LIN_ADVANCE SMOOTH_LIN_ADVANCE NONLINEAR_EXTRUSION INPUT_SHAPING_X INPUT_SHAPING_Y + INPUT_SHAPING_X INPUT_SHAPING_Y FT_MOTION FT_MOTION_MENU FTM_RESONANCE_TEST EMERGENCY_PARSER \ + BIQU_MICROPROBE_V1 PROBE_ENABLE_DISABLE Z_SAFE_HOMING AUTO_BED_LEVELING_BILINEAR \ + ADAPTIVE_STEP_SMOOTHING LIN_ADVANCE SMOOTH_LIN_ADVANCE NONLINEAR_EXTRUSION exec_test $1 $2 "BigTreeTech SKR Mini E3 1.0 - TMC2209 HW Serial, FT_MOTION" "$3" diff --git a/ini/features.ini b/ini/features.ini index f727f00f16..1c10302fb0 100644 --- a/ini/features.ini +++ b/ini/features.ini @@ -311,8 +311,9 @@ HAS_DUPLICATION_MODE = build_src_filter=+ PLATFORM_M997_SUPPORT = build_src_filter=+ HAS_TOOLCHANGE = build_src_filter=+ -FT_MOTION = build_src_filter=+ + - + +FT_MOTION = build_src_filter=+ + - + - FTM_SMOOTHING = build_src_filter=+ +FTM_RESONANCE_TEST = build_src_filter=+ HAS_LIN_ADVANCE_K = build_src_filter=+ PHOTO_GCODE = build_src_filter=+ CONTROLLER_FAN_EDITABLE = build_src_filter=+