diff --git a/Marlin/src/feature/resonance/resonance_generator.cpp b/Marlin/src/feature/resonance/resonance_generator.cpp index 5ce5d217f3..1e8414b9b2 100644 --- a/Marlin/src/feature/resonance/resonance_generator.cpp +++ b/Marlin/src/feature/resonance/resonance_generator.cpp @@ -24,55 +24,102 @@ #if ENABLED(RESONANCE_TEST) -#include "../../module/ft_motion.h" +#if ENABLED(FT_MOTION) + #include "../../module/ft_motion.h" +#endif + +#if HAS_STANDARD_MOTION + #include "../../module/stepper.h" +#endif + #include "resonance_generator.h" #include "../../gcode/gcode.h" // for home_all_axes -#include - 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::rt_time; // Resonance test timer float ResonanceGenerator::timeline = 0.0f; float ResonanceGenerator::amplitude_precalc; -float ResonanceGenerator::phase = 0.0f; float ResonanceGenerator::freq_mul; +float ResonanceGenerator::phase = 0.0f; + +#if HAS_STANDARD_MOTION + block_t ResonanceGenerator::block; +#endif ResonanceGenerator rtg; ResonanceGenerator::ResonanceGenerator() {} void ResonanceGenerator::start() { - gcode.home_all_axes(); // For safety and ensure known axes - - // Safe Acceleration per Hz for Z axis - if (rt_params.axis == Z_AXIS && rt_params.accel_per_hz > 15.0f) - rt_params.accel_per_hz = 15.0f; + gcode.home_all_axes(); // Always home axes first // Always move to the center of the bed do_blocking_move_to_xy(X_CENTER, Y_CENTER, Z_CLEARANCE_FOR_HOMING); rt_params.start_pos = current_position; - rt_time = FTM_TS; active = true; done = false; + + #if ENABLED(FT_MOTION) + if (ftMotion.cfg.active) + rt_time = FTM_TS; + #if HAS_STANDARD_MOTION + else + rt_time = 0.001f; + #endif + #else + rt_time = 0.001f; + #endif + + // Safe Acceleration per Hz for Z axis + if (rt_params.axis == Z_AXIS && rt_params.accel_per_hz > 15.0f) + rt_params.accel_per_hz = 15.0f; + + #if HAS_STANDARD_MOTION + if (TERN1(FT_MOTION, !ftMotion.cfg.active)){ + // Constant speed to be adjusted if necessary + block.reset(); + block.initial_rate = (rt_params.axis == Z_AXIS) ? 2000 : 8000; + } + #endif + // Precompute sine sweep const amplitude_precalc = (rt_params.amplitude_correction * rt_params.accel_per_hz * 0.25f) / sq(M_PI); current_freq = rt_params.min_freq; - const float inv_octave_duration = 1.0f / rt_params.octave_duration; - freq_mul = exp2f(FTM_TS * inv_octave_duration); + freq_mul = exp2f(rt_time / rt_params.octave_duration); } void ResonanceGenerator::abort() { reset(); - ftMotion.reset(); + #if(HAS_STANDARD_MOTION) + if (!TERN0(FT_MOTION, ftMotion.cfg.active)) + return; + #endif + #if ENABLED(FT_MOTION) + ftMotion.reset(); + #endif } void ResonanceGenerator::reset() { rt_params = resonance_test_params_t(); - rt_time = FTM_TS; + + #if ENABLED(FT_MOTION) + if (ftMotion.cfg.active) { + rt_time = FTM_TS; + } + #if HAS_STANDARD_MOTION + else { + rt_time = 0.001f; + block.reset(); + } + #endif + #else + rt_time = 0.001f; + block.reset(); + #endif active = false; done = false; } @@ -92,23 +139,59 @@ float ResonanceGenerator::calc_next_pos() { return rt_params.start_pos[rt_params.axis] + amplitude * r * (1.0f - 0.101321184f * r2); } -void ResonanceGenerator::fill_stepper_plan_buffer() { - xyze_float_t traj_coords = rt_params.start_pos; +#if ENABLED(FT_MOTION) + void ResonanceGenerator::fill_stepper_plan_buffer() { + xyze_pos_t traj_coords = rt_params.start_pos; - while (!ftMotion.stepping.is_full()) { + while (!ftMotion.stepping.is_full()) { + // Calculate current frequency + current_freq *= freq_mul; + if (current_freq > rt_params.max_freq) { + done = true; + return; + } + + // Resonate the axis being tested + traj_coords[rt_params.axis] = calc_next_pos(); + + // Store in buffer + ftMotion.stepping_enqueue(traj_coords); + } + } +#endif + +#if HAS_STANDARD_MOTION + block_t *ResonanceGenerator::generate_resonance_block() { + const float step_mm = planner.settings.axis_steps_per_mm[rt_params.axis]; + static float last_pos = 0.0f; + // Calculate current frequency current_freq *= freq_mul; if (current_freq > rt_params.max_freq) { done = true; - return; + return nullptr; } + + // Calculate next point position + const float target_pos = calc_next_pos(); - // Resonate the axis being tested - traj_coords[rt_params.axis] = calc_next_pos(); + // mm → steps + const float delta_mm = (target_pos - last_pos); + const int32_t delta_steps = abs(lround(delta_mm * step_mm)); + last_pos = target_pos; + // Steps for target axis + block.steps[rt_params.axis] = delta_steps; + // Total event count + block.step_event_count = delta_steps; - // Store in buffer - ftMotion.stepping_enqueue(traj_coords); - } -} + // Direction + if (delta_mm > 0) + block.direction_bits &= ~(1 << rt_params.axis); + else + block.direction_bits |= (1 << rt_params.axis); + + return █ + } +#endif #endif // RESONANCE_TEST diff --git a/Marlin/src/feature/resonance/resonance_generator.h b/Marlin/src/feature/resonance/resonance_generator.h index 31bf581b76..733b7377d4 100644 --- a/Marlin/src/feature/resonance/resonance_generator.h +++ b/Marlin/src/feature/resonance/resonance_generator.h @@ -25,6 +25,10 @@ #include +#if HAS_STANDARD_MOTION +#include "../../module/planner.h" +#endif + #ifndef M_TAU #define M_TAU (2.0f * M_PI) #endif @@ -41,7 +45,7 @@ typedef struct ResonanceTestParams { class ResonanceGenerator { public: - static resonance_test_params_t rt_params; // Resonance test parameters + static resonance_test_params_t rt_params; // Resonance test parameters static float timeline; // Timeline Value to calculate resonance frequency ResonanceGenerator(); @@ -56,7 +60,13 @@ class ResonanceGenerator { return rt_params.min_freq * exp2f(timeline / rt_params.octave_duration); } - void fill_stepper_plan_buffer(); // Fill stepper plan buffer with trajectory points + #if HAS_STANDARD_MOTION + block_t *generate_resonance_block(); // Generate planner block for standard motion + #endif + + #if ENABLED(FT_MOTION) + void fill_stepper_plan_buffer(); // Fill stepper plan buffer with trajectory points + #endif void setActive(const bool state) { active = state; } bool isActive() const { return active; } @@ -67,12 +77,15 @@ class ResonanceGenerator { void abort(); // Abort resonance test private: - float calc_next_pos(); // Calculate next position + float calc_next_pos(); // Calculate next position point based on current frequency static float rt_time; // Test timer static float freq_mul; // Frequency multiplier for sine sweeping static float amplitude_precalc; // Precalculated part of amplitude formula float current_freq; // Current frequency being generated in sinusoidal motion static float phase; // Current phase in radians + #if HAS_STANDARD_MOTION + static block_t block; + #endif static bool active; // Resonance test active static bool done; // Resonance test done }; diff --git a/Marlin/src/gcode/feature/resonance/M495_M496.cpp b/Marlin/src/gcode/feature/resonance/M495_M496.cpp index 68979c7ecb..638c9f3059 100644 --- a/Marlin/src/gcode/feature/resonance/M495_M496.cpp +++ b/Marlin/src/gcode/feature/resonance/M495_M496.cpp @@ -26,7 +26,6 @@ #include "../../gcode.h" #include "../../../lcd/marlinui.h" -#include "../../../module/ft_motion.h" #include "../../../feature/resonance/resonance_generator.h" void say_resonance_test() { @@ -146,23 +145,18 @@ void GcodeSuite::M495() { } 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")); - rtg.start(); - // The function returns immediately, the test runs in the background. - } - else { - SERIAL_ECHOLNPGM("?End Frequency must be greater than Start Frequency"); - } + if (p.axis != NO_AXIS_ENUM) { + if (p.max_freq > p.min_freq) { + SERIAL_ECHOLN(F("Starting "), F("Resonance Test")); + rtg.start(); + // The function returns immediately, the test runs in the background. } else { - SERIAL_ECHOLN(F("?Specify X, Y, or Z axis"), F(" first")); + SERIAL_ECHOLNPGM("?End Frequency must be greater than Start Frequency"); } } else { - SERIAL_ECHOLN(F("?Activate FT Motion to run the "), F("Resonance Test")); + SERIAL_ECHOLN(F("?Specify X, Y, or Z axis"), F(" first")); } } } diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index 3237a0b63d..386e343002 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -559,7 +559,6 @@ void menu_move() { EDIT_ITEM(bool, MSG_FTM_AXIS_SYNC, &editable.state, []{ queue.inject(TS(F("M493"), IAXIS_CHAR(MenuItemBase::itemIndex), 'T', int(editable.state))); }); - } END_MENU(); diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index d9da13bcaa..14e3cb796e 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -81,6 +81,10 @@ Stepper stepper; // Singleton #include "ft_motion.h" #endif +#if (ENABLED(RESONANCE_TEST) && HAS_STANDARD_MOTION) + #include "../feature/resonance/resonance_generator.h" +#endif + #include "../lcd/marlinui.h" #include "../gcode/queue.h" #include "../sd/cardreader.h" @@ -618,6 +622,103 @@ bool Stepper::disable_axis(const AxisEnum axis) { return can_disable; } +#if (ENABLED(RESONANCE_TEST) && HAS_STANDARD_MOTION) + hal_timer_t Stepper::resonance_block_phase_isr() { + + const hal_timer_t time_spent = HAL_timer_get_count(MF_TIMER_STEP); + #if MULTISTEPPING_LIMIT > 1 + if (steps_per_isr > 1 && time_spent_out_isr >= time_spent_in_isr + time_spent) + steps_per_isr >>= 1; + #endif + time_spent_in_isr = -time_spent; // Unsigned but guaranteed to be +ve when needed + time_spent_out_isr = 0; + + hal_timer_t interval = 0; + + // If current block is finished, reset pointer and finalize state + if (step_events_completed >= step_event_count) + discard_current_block(); + else + return calc_multistep_timer_interval(current_block->initial_rate); + + // If there is no current block at this point, generate a new block + if (!current_block){ + if ((current_block = rtg.generate_resonance_block())) { + + //Apply direction + DIR_WAIT_BEFORE(); + const uint8_t axis = rtg.rt_params.axis; + const bool fwd = current_block->direction_bits[axis]; + switch (axis) { + case X_AXIS: X_APPLY_DIR(fwd, false); + break; + case Y_AXIS: Y_APPLY_DIR(fwd, false); + break; + case Z_AXIS: Z_APPLY_DIR(fwd, false); + break; + } + + step_event_count = current_block->step_event_count; + + // No step events completed so far + step_events_completed = 0; + interval = calc_multistep_timer_interval(current_block->initial_rate); + } + else + rtg.abort(); + } + // Return the interval to wait + return interval; + } + + void Stepper::resonance_pulse_phase_isr() { + + // If there is no current block, do nothing + if (!current_block || step_events_completed >= step_event_count) return; + + // Count of pending loops and events for this iteration + const uint32_t pending_events = step_event_count - step_events_completed; + uint8_t events_to_do = _MIN(pending_events, steps_per_isr); + + // Just update the value we will get at the end of the loop + step_events_completed += events_to_do; + + USING_TIMED_PULSE(); + + // Take multiple steps per interrupt. For high speed moves. + bool firstStep = true; + const uint8_t axis = rtg.rt_params.axis; + + do { + + if (firstStep) + firstStep = false; + + switch (axis) { + case X_AXIS: + X_APPLY_STEP(STEP_STATE_X, false); + START_TIMED_PULSE(); + AWAIT_HIGH_PULSE(); + X_APPLY_STEP(!STEP_STATE_X, false); + break; + case Y_AXIS: + Y_APPLY_STEP(STEP_STATE_Y, false); + START_TIMED_PULSE(); + AWAIT_HIGH_PULSE(); + Y_APPLY_STEP(!STEP_STATE_Y, false); + break; + case Z_AXIS: + Z_APPLY_STEP(STEP_STATE_Z, false); + START_TIMED_PULSE(); + AWAIT_HIGH_PULSE(); + Z_APPLY_STEP(!STEP_STATE_Z, false); + break; + } + + } while (--events_to_do); + } +#endif + #if HAS_EXTRUDERS void Stepper::enable_extruder(E_TERN_(const uint8_t eindex)) { @@ -1646,6 +1747,19 @@ void Stepper::isr() { #if HAS_STANDARD_MOTION if (!using_ftMotion) { + #if ENABLED(RESONANCE_TEST) + if (rtg.isActive()) { + if (!nextMainISR) resonance_pulse_phase_isr(); + + hal.isr_on(); + if (!nextMainISR) nextMainISR = resonance_block_phase_isr(); + + interval = hal_timer_t(STEPPER_TIMER_RATE * 0.03); // Max wait of 30ms regardless of stepper timer frequency + NOMORE(interval, nextMainISR); // Time until the next Pulse / Block phase + nextMainISR -= interval; + } + else { + #endif TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed @@ -1709,6 +1823,7 @@ void Stepper::isr() { TERN_(BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval); } + TERN_(RESONANCE_TEST, }) #endif // HAS_STANDARD_MOTION diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 3d9820a496..7bbbbd8a89 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -579,6 +579,11 @@ class Stepper { static hal_timer_t block_phase_isr(); #endif + #if (ENABLED(RESONANCE_TEST) && HAS_STANDARD_MOTION) + static void resonance_pulse_phase_isr(); + static hal_timer_t resonance_block_phase_isr(); + #endif + #if HAS_ZV_SHAPING static void shaping_isr(); #endif