From 2d609487acd16e670f8e077ad8f1b9716b15b874 Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 16 Aug 2024 14:33:38 -0500 Subject: [PATCH] =?UTF-8?q?=F0=9F=91=B7=20FT=20Motion=20refactor,=20minor?= =?UTF-8?q?=20fix?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Marlin/src/gcode/feature/ft_motion/M493.cpp | 24 +++---- Marlin/src/inc/SanityCheck.h | 2 +- Marlin/src/lcd/menu/menu_motion.cpp | 23 +++---- Marlin/src/module/ft_motion.cpp | 69 +++++++++++---------- Marlin/src/module/ft_motion.h | 44 ++++++------- Marlin/src/module/ft_types.h | 21 ++++++- 6 files changed, 101 insertions(+), 82 deletions(-) diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index a9dab22852f..0229306f4b4 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -98,18 +98,18 @@ void say_shaping() { #if HAS_X_AXIS SERIAL_ECHO_TERNARY(dynamic, AXIS_0_NAME " ", "base dynamic", "static", " shaper frequency: "); - SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[X_AXIS], 2), F("Hz")); + SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq.x, 2), F("Hz")); #if HAS_DYNAMIC_FREQ - if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK[X_AXIS], 2), F("Hz/"), z_based ? F("mm") : F("g")); + if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK.x, 2), F("Hz/"), z_based ? F("mm") : F("g")); #endif SERIAL_EOL(); #endif #if HAS_Y_AXIS SERIAL_ECHO_TERNARY(dynamic, AXIS_1_NAME " ", "base dynamic", "static", " shaper frequency: "); - SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[Y_AXIS], 2), F(" Hz")); + SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq.y, 2), F(" Hz")); #if HAS_DYNAMIC_FREQ - if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK[Y_AXIS], 2), F("Hz/"), z_based ? F("mm") : F("g")); + if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK.y, 2), F("Hz/"), z_based ? F("mm") : F("g")); #endif SERIAL_EOL(); #endif @@ -131,17 +131,17 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { const ft_config_t &c = ftMotion.cfg; SERIAL_ECHOPGM(" M493 S", c.active); #if HAS_X_AXIS - SERIAL_ECHOPGM(" A", c.baseFreq[X_AXIS]); + SERIAL_ECHOPGM(" A", c.baseFreq.x); #if HAS_Y_AXIS - SERIAL_ECHOPGM(" B", c.baseFreq[Y_AXIS]); + SERIAL_ECHOPGM(" B", c.baseFreq.y); #endif #endif #if HAS_DYNAMIC_FREQ SERIAL_ECHOPGM(" D", c.dynFreqMode); #if HAS_X_AXIS - SERIAL_ECHOPGM(" F", c.dynFreqK[X_AXIS]); + SERIAL_ECHOPGM(" F", c.dynFreqK.x); #if HAS_Y_AXIS - SERIAL_ECHOPGM(" H", c.dynFreqK[Y_AXIS]); + SERIAL_ECHOPGM(" H", c.dynFreqK.y); #endif #endif #endif @@ -308,7 +308,7 @@ void GcodeSuite::M493() { const float val = parser.value_float(); // TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct. if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) { - ftMotion.cfg.baseFreq[X_AXIS] = val; + ftMotion.cfg.baseFreq.x = val; flag.update = flag.reset_ft = flag.report_h = true; } else // Frequency out of range. @@ -322,7 +322,7 @@ void GcodeSuite::M493() { // Parse frequency scaling parameter (X axis). if (parser.seenval('F')) { if (modeUsesDynFreq) { - ftMotion.cfg.dynFreqK[X_AXIS] = parser.value_float(); + ftMotion.cfg.dynFreqK.x = parser.value_float(); flag.report_h = true; } else @@ -369,7 +369,7 @@ void GcodeSuite::M493() { if (AXIS_HAS_SHAPER(Y)) { const float val = parser.value_float(); if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) { - ftMotion.cfg.baseFreq[Y_AXIS] = val; + ftMotion.cfg.baseFreq.y = val; flag.update = flag.reset_ft = flag.report_h = true; } else // Frequency out of range. @@ -383,7 +383,7 @@ void GcodeSuite::M493() { // Parse frequency scaling parameter (Y axis). if (parser.seenval('H')) { if (modeUsesDynFreq) { - ftMotion.cfg.dynFreqK[Y_AXIS] = parser.value_float(); + ftMotion.cfg.dynFreqK.y = parser.value_float(); flag.report_h = true; } else diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index e954df4a0c1..b61fb0e4428 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4381,7 +4381,7 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." #error "FT_MOTION requires FTM_UNIFIED_BWS to be enabled because FBS is not yet implemented." #endif #if !HAS_X_AXIS - static_assert(FTM_DEFAULT_X_COMPENSATOR != ftMotionShaper_NONE, "Without any linear axes FTM_DEFAULT_X_COMPENSATOR must be ftMotionShaper_NONE."); + static_assert(FTM_DEFAULT_SHAPER_X != ftMotionShaper_NONE, "Without any linear axes FTM_DEFAULT_SHAPER_X must be ftMotionShaper_NONE."); #endif #if HAS_DYNAMIC_FREQ_MM static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis."); diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index 5ec2948129f..ac53e01c112 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -358,7 +358,7 @@ void menu_move() { } inline void menu_ftm_cmpn_x() { - const ftMotionShaper_t shaper = ftMotion.cfg.shaper[X_AXIS]; + const ftMotionShaper_t shaper = ftMotion.cfg.shaper.x; START_MENU(); BACK_ITEM(MSG_FIXED_TIME_MOTION); @@ -376,7 +376,7 @@ void menu_move() { } inline void menu_ftm_cmpn_y() { - const ftMotionShaper_t shaper = ftMotion.cfg.shaper[Y_AXIS]; + const ftMotionShaper_t shaper = ftMotion.cfg.shaper.y; START_MENU(); BACK_ITEM(MSG_FIXED_TIME_MOTION); @@ -442,10 +442,10 @@ void menu_move() { MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(shaper_name[X_AXIS]); MENU_ITEM_ADDON_END(); if (AXIS_HAS_SHAPER(X)) { - EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[X_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params); - EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta[0], 0.0f, 1.0f, ftMotion.update_shaping_params); + EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq.x, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params); + EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta.x, 0.0f, 1.0f, ftMotion.update_shaping_params); if (AXIS_HAS_EISHAPER(X)) - EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol[0], 0.0f, 1.0f, ftMotion.update_shaping_params); + EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol.x, 0.0f, 1.0f, ftMotion.update_shaping_params); } #endif #if HAS_Y_AXIS @@ -453,10 +453,10 @@ void menu_move() { MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(shaper_name[Y_AXIS]); MENU_ITEM_ADDON_END(); if (AXIS_HAS_SHAPER(Y)) { - EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[Y_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params); - EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta[1], 0.0f, 1.0f, ftMotion.update_shaping_params); + EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq.y, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params); + EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta.y, 0.0f, 1.0f, ftMotion.update_shaping_params); if (AXIS_HAS_EISHAPER(Y)) - EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol[1], 0.0f, 1.0f, ftMotion.update_shaping_params); + EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol.y, 0.0f, 1.0f, ftMotion.update_shaping_params); } #endif @@ -465,10 +465,10 @@ void menu_move() { MENU_ITEM_ADDON_START_RJ(11); lcd_put_u8str(dmode); MENU_ITEM_ADDON_END(); if (c.dynFreqMode != dynFreqMode_DISABLED) { #if HAS_X_AXIS - EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_DFREQ_K_N, &c.dynFreqK[X_AXIS], 0.0f, 20.0f); + EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_DFREQ_K_N, &c.dynFreqK.x, 0.0f, 20.0f); #endif #if HAS_Y_AXIS - EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_DFREQ_K_N, &c.dynFreqK[Y_AXIS], 0.0f, 20.0f); + EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_DFREQ_K_N, &c.dynFreqK.y, 0.0f, 20.0f); #endif } #endif @@ -492,8 +492,6 @@ void menu_move() { MString<20> dmode = get_dyn_freq_mode_name(); #endif - ft_config_t &c = ftMotion.cfg; - START_MENU(); #if HAS_X_AXIS @@ -514,7 +512,6 @@ void menu_move() { #endif END_MENU(); - } #endif // FT_MOTION_MENU diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index 85993b6b0c1..a94516478b9 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -134,7 +134,7 @@ void FTMotion::loop() { } while (!blockProcRdy && (stepper.current_block = planner.get_current_block())) { - if (stepper.current_block->is_sync()) { // Sync block? + if (stepper.current_block->is_sync()) { // Sync block? if (stepper.current_block->is_sync_pos()) // Position sync? Set the position. stepper._set_position(stepper.current_block->position); discard_planner_block_protected(); @@ -166,7 +166,7 @@ void FTMotion::loop() { discard_planner_block_protected(); // Check if the block needs to be runout: - if (!batchRdy && !planner.movesplanned()){ + if (!batchRdy && !planner.movesplanned()) { runoutBlock(); makeVector(); // Do an additional makeVector call to guarantee batchRdy set this loop. } @@ -196,7 +196,7 @@ void FTMotion::loop() { batchRdy = false; // Clear so makeVector() can resume generating points. } - // Interpolation. + // Interpolation (generation of step commands from fixed time trajectory). while (batchRdyForInterp && (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))) { convertToSteps(interpIdx); @@ -350,14 +350,14 @@ void FTMotion::loop() { void FTMotion::update_shaping_params() { #if HAS_X_AXIS if ((shaping.x.ena = AXIS_HAS_SHAPER(X))) { - shaping.x.set_axis_shaping_A(cfg.shaper[X_AXIS], cfg.zeta[X_AXIS], cfg.vtol[X_AXIS]); - shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], cfg.baseFreq[X_AXIS], cfg.zeta[X_AXIS]); + shaping.x.set_axis_shaping_A(cfg.shaper.x, cfg.zeta.x, cfg.vtol.x); + shaping.x.set_axis_shaping_N(cfg.shaper.x, cfg.baseFreq.x, cfg.zeta.x); } #endif #if HAS_Y_AXIS if ((shaping.y.ena = AXIS_HAS_SHAPER(Y))) { - shaping.y.set_axis_shaping_A(cfg.shaper[Y_AXIS], cfg.zeta[Y_AXIS], cfg.vtol[Y_AXIS]); - shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], cfg.baseFreq[Y_AXIS], cfg.zeta[Y_AXIS]); + shaping.y.set_axis_shaping_A(cfg.shaper.y, cfg.zeta.y, cfg.vtol.y); + shaping.y.set_axis_shaping_N(cfg.shaper.y, cfg.baseFreq.y, cfg.zeta.y); } #endif } @@ -407,27 +407,29 @@ void FTMotion::discard_planner_block_protected() { } } -// Sets up a pseudo block to allow motion to settle buffers to empty. This is -// called when the planner has only one block left. The buffers will be filled -// with the last commanded position by setting the startPosn block variable to -// the last position of the previous block and all ratios to zero such that no -// axes' positions are incremented. +/** + * Set up a pseudo block to allow motion to settle and buffers to empty. + * Called when the planner has one block left. The buffers will be filled + * with the last commanded position by setting the startPosn block variable to + * the last position of the previous block and all ratios to zero such that no + * axes' positions are incremented. + */ void FTMotion::runoutBlock() { startPosn = endPosn_prevBlock; ratio.reset(); - int32_t n_to_fill_batch = FTM_WINDOW_SIZE - makeVector_batchIdx; + const int32_t n_to_fill_batch = (FTM_WINDOW_SIZE) - makeVector_batchIdx; - // This line is to be modified for FBS use; do not optimize out. - int32_t n_to_settle_cmpnstr = (TERN_(HAS_X_AXIS, shaping.x.ena) || TERN_(HAS_Y_AXIS, shaping.y.ena )) ? FTM_ZMAX : 0; + // This line or function is to be modified for FBS use; do not optimize out. + const int32_t n_to_settle_shaper = num_samples_shaper_settle(); - int32_t n_to_fill_batch_after_settling = (n_to_settle_cmpnstr > n_to_fill_batch) ? - FTM_BATCH_SIZE - ((n_to_settle_cmpnstr - n_to_fill_batch) % FTM_BATCH_SIZE) : n_to_fill_batch - n_to_settle_cmpnstr; + const int32_t n_diff = n_to_settle_shaper - n_to_fill_batch, + n_to_fill_batch_after_settling = n_diff > 0 ? (FTM_BATCH_SIZE) - (n_diff % (FTM_BATCH_SIZE)) : -n_diff; - int32_t n_to_settle_and_fill_batch = n_to_settle_cmpnstr + n_to_fill_batch_after_settling; + const int32_t n_to_settle_and_fill_batch = n_to_settle_shaper + n_to_fill_batch_after_settling; - max_intervals = PROP_BATCHES * FTM_BATCH_SIZE + n_to_settle_and_fill_batch; + max_intervals = (PROP_BATCHES) * (FTM_BATCH_SIZE) + n_to_settle_and_fill_batch; blockProcRdy = true; } @@ -571,13 +573,13 @@ void FTMotion::makeVector() { accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase } - #define _FTM_TRAJ(A) traj.A[makeVector_batchIdx] = startPosn.A + ratio.A * dist; - LOGICAL_AXIS_MAP_LC(_FTM_TRAJ); + #define _SET_TRAJ(q) traj.q[makeVector_batchIdx] = startPosn.q + ratio.q * dist; + LOGICAL_AXIS_MAP_LC(_SET_TRAJ); #if HAS_EXTRUDERS if (cfg.linearAdvEna) { float dedt_adj = (traj.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS); - if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK * 0.0001f; + if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK; e_raw_z1 = traj.e[makeVector_batchIdx]; e_advanced_z1 += dedt_adj * (FTM_TS); @@ -590,18 +592,21 @@ void FTMotion::makeVector() { switch (cfg.dynFreqMode) { #if HAS_DYNAMIC_FREQ_MM - case dynFreqMode_Z_BASED: - if (traj.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed. + case dynFreqMode_Z_BASED: { + static float oldz = 0.0f; + const float z = traj.z[makeVector_batchIdx]; + if (z != oldz) { // Only update if Z changed. + oldz = z; #if HAS_X_AXIS - const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx]; - shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], _MAX(xf, FTM_MIN_SHAPE_FREQ), cfg.zeta[X_AXIS]); + const float xf = cfg.baseFreq.x + cfg.dynFreqK.x * z; + shaping.x.set_axis_shaping_N(cfg.shaper.x, _MAX(xf, FTM_MIN_SHAPE_FREQ), cfg.zeta.x); #endif #if HAS_Y_AXIS - const float yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx]; - shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], _MAX(yf, FTM_MIN_SHAPE_FREQ), cfg.zeta[Y_AXIS]); + const float yf = cfg.baseFreq.y + cfg.dynFreqK.y * z; + shaping.y.set_axis_shaping_N(cfg.shaper.y, _MAX(yf, FTM_MIN_SHAPE_FREQ), cfg.zeta.y); #endif } - break; + } break; #endif #if HAS_DYNAMIC_FREQ_G @@ -609,10 +614,10 @@ void FTMotion::makeVector() { // Update constantly. The optimization done for Z value makes // less sense for E, as E is expected to constantly change. #if HAS_X_AXIS - shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx], cfg.zeta[X_AXIS]); + shaping.x.set_axis_shaping_N(cfg.shaper.x, cfg.baseFreq.x + cfg.dynFreqK.x * traj.e[makeVector_batchIdx], cfg.zeta.x); #endif #if HAS_Y_AXIS - shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx], cfg.zeta[Y_AXIS]); + shaping.y.set_axis_shaping_N(cfg.shaper.y, cfg.baseFreq.y + cfg.dynFreqK.y * traj.e[makeVector_batchIdx], cfg.zeta.y); #endif break; #endif @@ -722,7 +727,7 @@ void FTMotion::convertToSteps(const uint32_t idx) { err_P += delta; // Set up step/dir bits for all axes - #define _COMMAND_RUN(AXIS) command_set[_AXIS(AXIS)](err_P[_AXIS(AXIS)], steps[_AXIS(AXIS)], cmd, _BV(FT_BIT_DIR_##AXIS), _BV(FT_BIT_STEP_##AXIS)); + #define _COMMAND_RUN(A) command_set[_AXIS(A)](err_P.A, steps.A, cmd, _BV(FT_BIT_DIR_##A), _BV(FT_BIT_STEP_##A)); LOGICAL_AXIS_MAP(_COMMAND_RUN); // Next circular buffer index diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index eb717a91db9..8d60552d906 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -36,25 +36,23 @@ #endif #endif -#define NUM_AXES_SHAPED TERN(HAS_Y_AXIS, 2, 1) - typedef struct FTConfig { bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else standard motion) #if HAS_X_AXIS - ftMotionShaper_t shaper[NUM_AXES_SHAPED] = // Shaper type - { FTM_DEFAULT_SHAPER_X OPTARG(HAS_Y_AXIS, FTM_DEFAULT_SHAPER_Y) }; - float baseFreq[NUM_AXES_SHAPED] = // Base frequency. [Hz] - { FTM_SHAPING_DEFAULT_X_FREQ OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) }; - float zeta[NUM_AXES_SHAPED] = // Damping factor - { FTM_SHAPING_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_ZETA_Y) }; - float vtol[NUM_AXES_SHAPED] = // Vibration Level - { FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) }; + ft_shaped_shaper_t shaper = // Shaper type + { SHAPED_ELEM(FTM_DEFAULT_SHAPER_X, FTM_DEFAULT_SHAPER_Y) }; + ft_shaped_float_t baseFreq = // Base frequency. [Hz] + { SHAPED_ELEM(FTM_SHAPING_DEFAULT_X_FREQ, FTM_SHAPING_DEFAULT_Y_FREQ) }; + ft_shaped_float_t zeta = // Damping factor + { SHAPED_ELEM(FTM_SHAPING_ZETA_X, FTM_SHAPING_ZETA_Y) }; + ft_shaped_float_t vtol = // Vibration Level + { SHAPED_ELEM(FTM_SHAPING_V_TOL_X, FTM_SHAPING_V_TOL_Y) }; #endif #if HAS_DYNAMIC_FREQ dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration. - float dynFreqK[NUM_AXES_SHAPED] = { 0.0f }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g] + ft_shaped_float_t dynFreqK = { 0.0f }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g] #else static constexpr dynFreqMode_t dynFreqMode = dynFreqMode_DISABLED; #endif @@ -77,22 +75,23 @@ class FTMotion { cfg.active = ENABLED(FTM_IS_DEFAULT_MOTION); #if HAS_X_AXIS - cfg.shaper[X_AXIS] = FTM_DEFAULT_SHAPER_X; - cfg.baseFreq[X_AXIS] = FTM_SHAPING_DEFAULT_X_FREQ; - cfg.zeta[X_AXIS] = FTM_SHAPING_ZETA_X; - cfg.vtol[X_AXIS] = FTM_SHAPING_V_TOL_X; + cfg.shaper.x = FTM_DEFAULT_SHAPER_X; + cfg.baseFreq.x = FTM_SHAPING_DEFAULT_X_FREQ; + cfg.zeta.x = FTM_SHAPING_ZETA_X; + cfg.vtol.x = FTM_SHAPING_V_TOL_X; #endif #if HAS_Y_AXIS - cfg.shaper[Y_AXIS] = FTM_DEFAULT_SHAPER_Y; - cfg.baseFreq[Y_AXIS] = FTM_SHAPING_DEFAULT_Y_FREQ; - cfg.zeta[Y_AXIS] = FTM_SHAPING_ZETA_Y; - cfg.vtol[Y_AXIS] = FTM_SHAPING_V_TOL_Y; + cfg.shaper.y = FTM_DEFAULT_SHAPER_Y; + cfg.baseFreq.y = FTM_SHAPING_DEFAULT_Y_FREQ; + cfg.zeta.y = FTM_SHAPING_ZETA_Y; + cfg.vtol.y = FTM_SHAPING_V_TOL_Y; #endif #if HAS_DYNAMIC_FREQ cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; - cfg.dynFreqK[X_AXIS] = TERN_(HAS_Y_AXIS, cfg.dynFreqK[Y_AXIS]) = 0.0f; + TERN_(HAS_X_AXIS, cfg.dynFreqK.x = 0.0f); + TERN_(HAS_Y_AXIS, cfg.dynFreqK.y = 0.0f); #endif #if HAS_EXTRUDERS @@ -143,7 +142,8 @@ class FTMotion { static uint32_t N1, N2, N3; static uint32_t max_intervals; - static constexpr uint32_t PROP_BATCHES = CEIL(FTM_WINDOW_SIZE/FTM_BATCH_SIZE) - 1; // Number of batches needed to propagate the current trajectory to the stepper. + // Number of batches needed to propagate the current trajectory to the stepper. + static constexpr uint32_t PROP_BATCHES = CEIL((FTM_WINDOW_SIZE) / (FTM_BATCH_SIZE)) - 1; // Make vector variables. static uint32_t makeVector_idx, @@ -195,7 +195,7 @@ class FTMotion { static void makeVector(); static void convertToSteps(const uint32_t idx); - FORCE_INLINE static int32_t num_samples_cmpnstr_settle() { return ( shaping.x.ena || shaping.y.ena ) ? FTM_ZMAX : 0; } + FORCE_INLINE static int32_t num_samples_shaper_settle() { return ( shaping.x.ena || shaping.y.ena ) ? FTM_ZMAX : 0; } }; // class FTMotion diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h index d43340b73b8..7659974409d 100644 --- a/Marlin/src/module/ft_types.h +++ b/Marlin/src/module/ft_types.h @@ -23,7 +23,7 @@ #include "../core/types.h" -typedef enum FXDTICtrlShaper : uint8_t { +enum ftMotionShaper_t : uint8_t { ftMotionShaper_NONE = 0, // No compensator ftMotionShaper_ZV = 1, // Zero Vibration ftMotionShaper_ZVD = 2, // Zero Vibration and Derivative @@ -33,7 +33,7 @@ typedef enum FXDTICtrlShaper : uint8_t { ftMotionShaper_2HEI = 6, // 2-Hump Extra-Intensive ftMotionShaper_3HEI = 7, // 3-Hump Extra-Intensive ftMotionShaper_MZV = 8 // Modified Zero Vibration -} ftMotionShaper_t; +}; enum dynFreqMode_t : uint8_t { dynFreqMode_DISABLED = 0, @@ -59,4 +59,21 @@ enum { FT_BIT_COUNT }; +#define NUM_AXES_SHAPED TERN(HAS_Y_AXIS, 2, 1) +#define SHAPED_ELEM(A, B) A OPTARG(HAS_Y_AXIS, B) + +template +struct FTShapedAxes { + union { + struct { T SHAPED_ELEM(X, Y); }; + struct { T SHAPED_ELEM(x, y); }; + T val[NUM_AXES_SHAPED]; + }; + T& operator[](int i) { return val[i]; } +}; + +typedef FTShapedAxes ft_shaped_float_t; +typedef FTShapedAxes ft_shaped_shaper_t; +typedef FTShapedAxes ft_shaped_dfm_t; + typedef bits_t(FT_BIT_COUNT) ft_command_t;