|
| 1 | +/** |
| 2 | + * Marlin 3D Printer Firmware |
| 3 | + * Copyright (c) 2026 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] |
| 4 | + * |
| 5 | + * Based on Sprinter and grbl. |
| 6 | + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm |
| 7 | + * |
| 8 | + * This program is free software: you can redistribute it and/or modify |
| 9 | + * it under the terms of the GNU General Public License as published by |
| 10 | + * the Free Software Foundation, either version 3 of the License, or |
| 11 | + * (at your option) any later version. |
| 12 | + * |
| 13 | + * This program is distributed in the hope that it will be useful, |
| 14 | + * but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 15 | + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| 16 | + * GNU General Public License for more details. |
| 17 | + * |
| 18 | + * You should have received a copy of the GNU General Public License |
| 19 | + * along with this program. If not, see <https://www.gnu.org/licenses/>. |
| 20 | + * |
| 21 | + */ |
| 22 | + |
| 23 | +#include "../../inc/MarlinConfigPre.h" |
| 24 | + |
| 25 | +#if ENABLED(RESONANCE_TEST) |
| 26 | + |
| 27 | +#include "resonance_generator.h" |
| 28 | + |
| 29 | +#if ENABLED(FT_MOTION) |
| 30 | + #include "../../module/ft_motion.h" |
| 31 | +#endif |
| 32 | + |
| 33 | +#include "../../gcode/gcode.h" // for home_all_axes |
| 34 | + |
| 35 | +resonance_test_params_t ResonanceGenerator::rt_params; // Resonance test parameters |
| 36 | +float ResonanceGenerator::timeline; |
| 37 | + |
| 38 | +bool ResonanceGenerator::active = false; // Resonance test active |
| 39 | +bool ResonanceGenerator::done = false; // Resonance test done |
| 40 | +int32_t ResonanceGenerator::freq_to_phase_fp; |
| 41 | +int32_t ResonanceGenerator::max_freq_fp; |
| 42 | +int32_t ResonanceGenerator::phase_fp; |
| 43 | +int32_t ResonanceGenerator::current_freq_fp; |
| 44 | +int32_t ResonanceGenerator::amplitude_precalc_fp; |
| 45 | + |
| 46 | + |
| 47 | +#if HAS_STANDARD_MOTION |
| 48 | + block_t ResonanceGenerator::block; |
| 49 | +#endif |
| 50 | + |
| 51 | +ResonanceGenerator rtg; |
| 52 | + |
| 53 | +ResonanceGenerator::ResonanceGenerator() {} |
| 54 | + |
| 55 | +void ResonanceGenerator::start() { |
| 56 | + gcode.home_all_axes(); // Always home axes first |
| 57 | + motion.blocking_move_xy(X_CENTER, Y_CENTER, Z_CLEARANCE_FOR_HOMING); |
| 58 | + |
| 59 | + rt_params.start_pos = motion.position; |
| 60 | + active = true; |
| 61 | + done = false; |
| 62 | + |
| 63 | + // Clamp Z-axis acceleration for safety |
| 64 | + if (rt_params.axis == Z_AXIS) |
| 65 | + NOMORE(rt_params.accel_per_hz, 15.0f); |
| 66 | + |
| 67 | + // Calculate time constant for sine sweep |
| 68 | + const float rt_time = rt_params.octave_duration * (logf(RATIO) / logf(2.0f)); |
| 69 | + |
| 70 | + #if HAS_STANDARD_MOTION |
| 71 | + if (TERN1(FT_MOTION, !ftMotion.cfg.active)) { |
| 72 | + block.reset(); |
| 73 | + block.initial_rate = (rt_params.axis == Z_AXIS) ? 2000 : 8000; |
| 74 | + } |
| 75 | + #endif |
| 76 | + |
| 77 | + // Precompute fixed-point sine sweep parameters |
| 78 | + amplitude_precalc_fp = F2FP((rt_params.amplitude_correction * rt_params.accel_per_hz * 0.25f) / sq(M_PI)); |
| 79 | + current_freq_fp = F2FP(rt_params.min_freq); |
| 80 | + freq_to_phase_fp = F2FP(2.0f * M_PI * rt_time); |
| 81 | + max_freq_fp = F2FP(rt_params.max_freq); |
| 82 | + phase_fp = 0; |
| 83 | +} |
| 84 | + |
| 85 | +void ResonanceGenerator::abort() { |
| 86 | + reset(); |
| 87 | + #if HAS_STANDARD_MOTION |
| 88 | + if (!TERN0(FT_MOTION, ftMotion.cfg.active)) |
| 89 | + return; |
| 90 | + #endif |
| 91 | + TERN_(FT_MOTION, ftMotion.reset()); |
| 92 | +} |
| 93 | + |
| 94 | +void ResonanceGenerator::reset() { |
| 95 | + rt_params = resonance_test_params_t(); |
| 96 | + #if HAS_STANDARD_MOTION |
| 97 | + if (!TERN0(FT_MOTION, ftMotion.cfg.active)) |
| 98 | + block.reset(); |
| 99 | + #endif |
| 100 | + active = false; |
| 101 | + done = false; |
| 102 | +} |
| 103 | + |
| 104 | +float ResonanceGenerator::calc_next_pos() { |
| 105 | + // Phase accumulation and wrapping within [0, 2π) |
| 106 | + phase_fp += (int32_t)(((int64_t)current_freq_fp * freq_to_phase_fp) >> FP_BITS); |
| 107 | + if (phase_fp >= M_TAU_FP) phase_fp -= M_TAU_FP; |
| 108 | + else if (phase_fp < 0) phase_fp += M_TAU_FP; |
| 109 | + |
| 110 | + // -π <= r_fp <= π |
| 111 | + const int32_t r_fp = (phase_fp > M_PI_FP) ? phase_fp - M_TAU_FP : phase_fp; |
| 112 | + |
| 113 | + // Calculate windowing polynomial: 1.0 - 0.101321184 * r² |
| 114 | + const int32_t poly_fp = FP_ONE - ((C0101321184_FP * ((r_fp * r_fp) >> FP_BITS)) >> FP_BITS); |
| 115 | + |
| 116 | + // Combine amplitude, phase, and polynomial and return new position |
| 117 | + const int32_t amplitude_fp = (int32_t)(((int64_t)amplitude_precalc_fp * FP_ONE) / current_freq_fp); |
| 118 | + const int32_t pos_fp = (int32_t)((((int64_t)amplitude_fp * r_fp) >> FP_BITS) * poly_fp) >> FP_BITS; |
| 119 | + |
| 120 | + return FP2F(pos_fp); |
| 121 | +} |
| 122 | + |
| 123 | +#if ENABLED(FT_MOTION) |
| 124 | + |
| 125 | + void ResonanceGenerator::fill_stepper_plan_buffer() { |
| 126 | + #if HAS_FTM_DIR_CHANGE_HOLD |
| 127 | + xyze_float_t traj_coords = ftMotion.get_last_target_traj(); |
| 128 | + traj_coords[rt_params.axis] = rt_params.start_pos[rt_params.axis]; |
| 129 | + #else |
| 130 | + xyze_float_t traj_coords = rt_params.start_pos; |
| 131 | + #endif |
| 132 | + // Save starting position, avoid cumulative errors |
| 133 | + const float start_pos = rt_params.start_pos[rt_params.axis]; |
| 134 | + |
| 135 | + while (!ftMotion.stepping.is_full()) { |
| 136 | + // Calculate current frequency with exponential sweep |
| 137 | + current_freq_fp += current_freq_fp >> FP_BITS; |
| 138 | + if (current_freq_fp > max_freq_fp) { |
| 139 | + done = true; |
| 140 | + return; |
| 141 | + } |
| 142 | + |
| 143 | + // Resonate the axis being tested |
| 144 | + traj_coords[rt_params.axis] = start_pos + calc_next_pos(); |
| 145 | + |
| 146 | + TERN_(HAS_FTM_DIR_CHANGE_HOLD, traj_coords = ftMotion.ftm_hold_frames(traj_coords)); |
| 147 | + |
| 148 | + // Store in buffer |
| 149 | + ftMotion.stepping_enqueue(traj_coords); |
| 150 | + } |
| 151 | + } |
| 152 | + |
| 153 | +#endif // FT_MOTION |
| 154 | + |
| 155 | +#if HAS_STANDARD_MOTION |
| 156 | + |
| 157 | + block_t* ResonanceGenerator::generate_resonance_block() { |
| 158 | + // Static variables to retain state between calls and avoid cumulative errors |
| 159 | + static float prev_pos = 0.0f, step_accumulator = 0.0f; |
| 160 | + |
| 161 | + const float step_mm = planner.settings.axis_steps_per_mm[rt_params.axis]; |
| 162 | + const uint8_t axis_bit = 1 << rt_params.axis; |
| 163 | + |
| 164 | + // Calculate current frequency with exponential sweep |
| 165 | + current_freq_fp += current_freq_fp >> FP_BITS; |
| 166 | + if (current_freq_fp > max_freq_fp) { |
| 167 | + done = true; |
| 168 | + return nullptr; |
| 169 | + } |
| 170 | + |
| 171 | + // Calculate position and accumulate steps |
| 172 | + const float current_pos = calc_next_pos(); |
| 173 | + step_accumulator += (current_pos - prev_pos) * step_mm; |
| 174 | + prev_pos = current_pos; |
| 175 | + |
| 176 | + // Extract steps |
| 177 | + const int32_t delta_steps = (int32_t)floor(step_accumulator); |
| 178 | + step_accumulator -= delta_steps; |
| 179 | + const uint32_t abs_steps = abs(delta_steps); |
| 180 | + |
| 181 | + // Update block |
| 182 | + block.steps[rt_params.axis] = abs_steps; |
| 183 | + block.step_event_count = abs_steps; |
| 184 | + if (delta_steps < 0) |
| 185 | + block.direction_bits |= axis_bit; |
| 186 | + else |
| 187 | + block.direction_bits &= ~axis_bit; |
| 188 | + |
| 189 | + return █ |
| 190 | + } |
| 191 | + |
| 192 | +#endif // HAS_STANDARD_MOTION |
| 193 | + |
| 194 | +#endif // RESONANCE_TEST |
0 commit comments