From f0bc4274f817166fcce82949d94330bd1c441c15 Mon Sep 17 00:00:00 2001 From: narno2202 <130909513+narno2202@users.noreply.github.com> Date: Mon, 15 Jul 2024 20:13:00 +0200 Subject: [PATCH] =?UTF-8?q?=F0=9F=A7=91=E2=80=8D=F0=9F=92=BB=20FT=20Motion?= =?UTF-8?q?:=20Individual=20axis=20shaping,=20new=20buffer=20management=20?= =?UTF-8?q?(#26848)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Scott Lahteine --- Marlin/Configuration_adv.h | 14 +- Marlin/src/HAL/ESP32/i2s.cpp | 2 +- Marlin/src/gcode/feature/ft_motion/M493.cpp | 180 ++++--- Marlin/src/inc/SanityCheck.h | 9 + Marlin/src/lcd/language/language_en.h | 2 +- Marlin/src/lcd/menu/menu_motion.cpp | 141 +++--- Marlin/src/module/endstops.cpp | 50 +- Marlin/src/module/ft_motion.cpp | 494 +++++++++----------- Marlin/src/module/ft_motion.h | 83 ++-- Marlin/src/module/ft_types.h | 26 +- Marlin/src/module/planner.cpp | 10 +- Marlin/src/module/stepper.cpp | 137 +----- Marlin/src/module/stepper.h | 2 - 13 files changed, 500 insertions(+), 650 deletions(-) diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index cf203c447d6..b816b230ade 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1118,11 +1118,14 @@ /** * Fixed-time-based Motion Control -- EXPERIMENTAL * Enable/disable and set parameters with G-code M493. + * See ft_types.h for named values used by FTM options. */ //#define FT_MOTION #if ENABLED(FT_MOTION) - #define FTM_DEFAULT_MODE ftMotionMode_DISABLED // Default mode of fixed time control. (Enums in ft_types.h) - #define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (Enums in ft_types.h) + //#define FTM_IS_DEFAULT_MOTION // Use FT Motion as the factory default? + #define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (DISABLED, Z_BASED, MASS_BASED) + #define FTM_DEFAULT_SHAPER_X ftMotionShaper_NONE // Default shaper mode on X axis (NONE, ZV, ZVD, ZVDD, ZVDDD, EI, 2HEI, 3HEI, MZV) + #define FTM_DEFAULT_SHAPER_Y ftMotionShaper_NONE // Default shaper mode on Y axis #define FTM_SHAPING_DEFAULT_X_FREQ 37.0f // (Hz) Default peak frequency used by input shapers #define FTM_SHAPING_DEFAULT_Y_FREQ 37.0f // (Hz) Default peak frequency used by input shapers #define FTM_LINEAR_ADV_DEFAULT_ENA false // Default linear advance enable (true) or disable (false) @@ -1149,18 +1152,13 @@ #define FTM_FS 1000 // (Hz) Frequency for trajectory generation. (Reciprocal of FTM_TS) #define FTM_TS 0.001f // (s) Time step for trajectory generation. (Reciprocal of FTM_FS) - // These values may be configured to adjust the duration of loop(). - #define FTM_STEPS_PER_LOOP 60 // Number of stepper commands to generate each loop() - #define FTM_POINTS_PER_LOOP 100 // Number of trajectory points to generate each loop() - #if DISABLED(COREXY) #define FTM_STEPPER_FS 20000 // (Hz) Frequency for stepper I/O update // Use this to adjust the time required to consume the command buffer. // Try increasing this value if stepper motion is choppy. #define FTM_STEPPERCMD_BUFF_SIZE 3000 // Size of the stepper command buffers - // (FTM_STEPS_PER_LOOP * FTM_POINTS_PER_LOOP) is a good start - // If you run out of memory, fall back to 3000 and increase progressively + #else // CoreXY motion needs a larger buffer size. These values are based on our testing. #define FTM_STEPPER_FS 30000 diff --git a/Marlin/src/HAL/ESP32/i2s.cpp b/Marlin/src/HAL/ESP32/i2s.cpp index 5404c3e9e2d..4b17db3daf7 100644 --- a/Marlin/src/HAL/ESP32/i2s.cpp +++ b/Marlin/src/HAL/ESP32/i2s.cpp @@ -152,7 +152,7 @@ void stepperTask(void *parameter) { xQueueReceive(dma.queue, &dma.current, portMAX_DELAY); dma.rw_pos = 0; - const bool using_ftMotion = TERN0(FT_MOTION, ftMotion.cfg.mode); + const bool using_ftMotion = TERN0(FT_MOTION, ftMotion.cfg.active); while (dma.rw_pos < DMA_SAMPLE_COUNT) { diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp index 2da92b8582d..993b4b20357 100644 --- a/Marlin/src/gcode/feature/ft_motion/M493.cpp +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -28,30 +28,51 @@ #include "../../../module/ft_motion.h" #include "../../../module/stepper.h" +void say_shaper_type(const AxisEnum a) { + SERIAL_ECHOPGM(" axis "); + switch (ftMotion.cfg.shaper[a]) { + default: break; + case ftMotionShaper_ZV: SERIAL_ECHOPGM("ZV"); break; + case ftMotionShaper_ZVD: SERIAL_ECHOPGM("ZVD"); break; + case ftMotionShaper_ZVDD: SERIAL_ECHOPGM("ZVDD"); break; + case ftMotionShaper_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break; + case ftMotionShaper_EI: SERIAL_ECHOPGM("EI"); break; + case ftMotionShaper_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break; + case ftMotionShaper_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break; + case ftMotionShaper_MZV: SERIAL_ECHOPGM("MZV"); break; + } + SERIAL_ECHOPGM(" shaping"); +} + +#if CORE_IS_XY || CORE_IS_XZ + #define AXIS_0_NAME "A" +#else + #define AXIS_0_NAME "X" +#endif +#if CORE_IS_XY || CORE_IS_YZ + #define AXIS_1_NAME "B" +#else + #define AXIS_1_NAME "Y" +#endif + void say_shaping() { // FT Enabled - SERIAL_ECHO_TERNARY(ftMotion.cfg.mode, "Fixed-Time Motion ", "en", "dis", "abled"); + SERIAL_ECHO_TERNARY(ftMotion.cfg.active, "Fixed-Time Motion ", "en", "dis", "abled"); // FT Shaping #if HAS_X_AXIS - if (ftMotion.cfg.mode > ftMotionMode_ENABLED) { - SERIAL_ECHOPGM(" with "); - switch (ftMotion.cfg.mode) { - default: break; - case ftMotionMode_ZV: SERIAL_ECHOPGM("ZV"); break; - case ftMotionMode_ZVD: SERIAL_ECHOPGM("ZVD"); break; - case ftMotionMode_ZVDD: SERIAL_ECHOPGM("ZVDD"); break; - case ftMotionMode_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break; - case ftMotionMode_EI: SERIAL_ECHOPGM("EI"); break; - case ftMotionMode_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break; - case ftMotionMode_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break; - case ftMotionMode_MZV: SERIAL_ECHOPGM("MZV"); break; - //case ftMotionMode_DISCTF: SERIAL_ECHOPGM("discrete transfer functions"); break; - //case ftMotionMode_ULENDO_FBS: SERIAL_ECHOPGM("Ulendo FBS."); return; - } - SERIAL_ECHOPGM(" shaping"); + if (CMPNSTR_HAS_SHAPER(X_AXIS)) { + SERIAL_ECHOPGM(" with " AXIS_0_NAME); + say_shaper_type(X_AXIS); } #endif + #if HAS_Y_AXIS + if (CMPNSTR_HAS_SHAPER(Y_AXIS)) { + SERIAL_ECHOPGM(" and with " AXIS_1_NAME); + say_shaper_type(Y_AXIS); + } + #endif + SERIAL_ECHOLNPGM("."); const bool z_based = TERN0(HAS_DYNAMIC_FREQ_MM, ftMotion.cfg.dynFreqMode == dynFreqMode_Z_BASED), @@ -59,7 +80,7 @@ void say_shaping() { dynamic = z_based || g_based; // FT Dynamic Frequency Mode - if (ftMotion.cfg.modeHasShaper()) { + if (CMPNSTR_HAS_SHAPER(X_AXIS) || CMPNSTR_HAS_SHAPER(Y_AXIS)) { #if HAS_DYNAMIC_FREQ SERIAL_ECHOPGM("Dynamic Frequency Mode "); switch (ftMotion.cfg.dynFreqMode) { @@ -76,7 +97,7 @@ void say_shaping() { #endif #if HAS_X_AXIS - SERIAL_ECHO_TERNARY(dynamic, "X/A ", "base dynamic", "static", " compensator frequency: "); + 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")); #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")); @@ -85,7 +106,7 @@ void say_shaping() { #endif #if HAS_Y_AXIS - SERIAL_ECHO_TERNARY(dynamic, "Y/B ", "base dynamic", "static", " compensator frequency: "); + 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")); #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")); @@ -108,7 +129,7 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { report_heading_etc(forReplay, F(STR_FT_MOTION)); const ft_config_t &c = ftMotion.cfg; - SERIAL_ECHOPGM(" M493 S", c.mode); + SERIAL_ECHOPGM(" M493 S", c.active); #if HAS_X_AXIS SERIAL_ECHOPGM(" A", c.baseFreq[X_AXIS]); #if HAS_Y_AXIS @@ -133,18 +154,21 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { /** * M493: Set Fixed-time Motion Control parameters * - * S Set the motion / shaping mode. Shaping requires an X axis, at the minimum. + * S Set Fixed-Time motion mode on or off. + * 0: Fixed-Time Motion OFF (Standard Motion) + * 1: Fixed-Time Motion ON * - * 0: Standard Motion - * 1: Fixed-Time Motion - * 10: ZV : Zero Vibration - * 11: ZVD : Zero Vibration and Derivative - * 12: ZVDD : Zero Vibration, Derivative, and Double Derivative - * 13: ZVDDD : Zero Vibration, Derivative, Double Derivative, and Triple Derivative - * 14: EI : Extra-Intensive - * 15: 2HEI : 2-Hump Extra-Intensive - * 16: 3HEI : 3-Hump Extra-Intensive - * 17: MZV : Mass-based Zero Vibration + * X/Y Set the vibration compensator [input shaper] mode for X / Y axis. + * Users / slicers must remember to set the mode for both axes! + * 0: NONE : No input shaper + * 1: ZV : Zero Vibration + * 2: ZVD : Zero Vibration and Derivative + * 3: ZVDD : Zero Vibration, Derivative, and Double Derivative + * 4: ZVDDD : Zero Vibration, Derivative, Double Derivative, and Triple Derivative + * 5: EI : Extra-Intensive + * 6: 2HEI : 2-Hump Extra-Intensive + * 7: 3HEI : 3-Hump Extra-Intensive + * 8: MZV : Mass-based Zero Vibration * * P Enable (1) or Disable (0) Linear Advance pressure control * @@ -166,40 +190,56 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) { * R 0.00 Set the vibration tolerance for the Y axis */ void GcodeSuite::M493() { - struct { bool update_n:1, update_a:1, reset_ft:1, report_h:1; } flag = { false }; + struct { bool update:1, reset_ft:1, report_h:1; } flag = { false }; if (!parser.seen_any()) flag.report_h = true; // Parse 'S' mode parameter. - if (parser.seenval('S')) { - const ftMotionMode_t newmm = (ftMotionMode_t)parser.value_byte(); + if (parser.seen('S')) { + const bool active = parser.value_bool(); - if (newmm != ftMotion.cfg.mode) { - switch (newmm) { - default: SERIAL_ECHOLNPGM("?Invalid control mode [S] value."); return; - #if HAS_X_AXIS - case ftMotionMode_ZV: - case ftMotionMode_ZVD: - case ftMotionMode_ZVDD: - case ftMotionMode_ZVDDD: - case ftMotionMode_EI: - case ftMotionMode_2HEI: - case ftMotionMode_3HEI: - case ftMotionMode_MZV: - //case ftMotionMode_ULENDO_FBS: - //case ftMotionMode_DISCTF: - flag.update_n = flag.update_a = true; - #endif - case ftMotionMode_DISABLED: flag.reset_ft = true; - case ftMotionMode_ENABLED: - ftMotion.cfg.mode = newmm; - flag.report_h = true; + if (active != ftMotion.cfg.active) { + switch (active) { + case false: flag.reset_ft = true; + case true: flag.report_h = true; + ftMotion.cfg.active = active; break; } } } + #if HAS_X_AXIS + auto set_shaper = [&](const AxisEnum axis, const char c) { + const ftMotionShaper_t newsh = (ftMotionShaper_t)parser.value_byte(); + if (newsh != ftMotion.cfg.shaper[axis]) { + switch (newsh) { + default: SERIAL_ECHOLNPGM("?Invalid [", C(c), "] shaper."); return true; + case ftMotionShaper_NONE: + case ftMotionShaper_ZV: + case ftMotionShaper_ZVD: + case ftMotionShaper_ZVDD: + case ftMotionShaper_ZVDDD: + case ftMotionShaper_EI: + case ftMotionShaper_2HEI: + case ftMotionShaper_3HEI: + case ftMotionShaper_MZV: + ftMotion.cfg.shaper[axis] = newsh; + flag.update = flag.report_h = true; + break; + } + } + return false; + }; + + if (parser.seenval('X') && set_shaper(X_AXIS, 'X')) return; // Parse 'X' mode parameter + + #if HAS_Y_AXIS + if (parser.seenval('Y') && set_shaper(Y_AXIS, 'Y')) return; // Parse 'Y' mode parameter + #endif + + #endif // HAS_X_AXIS + #if HAS_EXTRUDERS // Pressure control (linear advance) parameter. @@ -227,7 +267,7 @@ void GcodeSuite::M493() { // Dynamic frequency mode parameter. if (parser.seenval('D')) { - if (ftMotion.cfg.modeHasShaper()) { + if (CMPNSTR_HAS_SHAPER(X_AXIS) || CMPNSTR_HAS_SHAPER(Y_AXIS)) { const dynFreqMode_t val = dynFreqMode_t(parser.value_byte()); switch (val) { #if HAS_DYNAMIC_FREQ_MM @@ -261,12 +301,12 @@ void GcodeSuite::M493() { // Parse frequency parameter (X axis). if (parser.seenval('A')) { - if (ftMotion.cfg.modeHasShaper()) { + if (CMPNSTR_HAS_SHAPER(X_AXIS)) { 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; - flag.update_n = flag.reset_ft = flag.report_h = true; + flag.update = flag.reset_ft = flag.report_h = true; } else // Frequency out of range. SERIAL_ECHOLNPGM("Invalid [", C('A'), "] frequency value."); @@ -290,10 +330,10 @@ void GcodeSuite::M493() { // Parse zeta parameter (X axis). if (parser.seenval('I')) { const float val = parser.value_float(); - if (ftMotion.cfg.modeHasShaper()) { + if (CMPNSTR_HAS_SHAPER(X_AXIS)) { if (WITHIN(val, 0.01f, 1.0f)) { ftMotion.cfg.zeta[0] = val; - flag.update_n = flag.update_a = true; + flag.update = true; } else SERIAL_ECHOLNPGM("Invalid X zeta [", C('I'), "] value."); // Zeta out of range. @@ -305,10 +345,10 @@ void GcodeSuite::M493() { // Parse vtol parameter (X axis). if (parser.seenval('Q')) { const float val = parser.value_float(); - if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) { + if (CMPNSTR_IS_EISHAPER(X_AXIS)) { if (WITHIN(val, 0.00f, 1.0f)) { ftMotion.cfg.vtol[0] = val; - flag.update_a = true; + flag.update = true; } else SERIAL_ECHOLNPGM("Invalid X vtol [", C('Q'), "] value."); // VTol out of range. @@ -323,11 +363,11 @@ void GcodeSuite::M493() { // Parse frequency parameter (Y axis). if (parser.seenval('B')) { - if (ftMotion.cfg.modeHasShaper()) { + if (CMPNSTR_HAS_SHAPER(Y_AXIS)) { const float val = parser.value_float(); if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) { ftMotion.cfg.baseFreq[Y_AXIS] = val; - flag.update_n = flag.reset_ft = flag.report_h = true; + flag.update = flag.reset_ft = flag.report_h = true; } else // Frequency out of range. SERIAL_ECHOLNPGM("Invalid frequency [", C('B'), "] value."); @@ -351,10 +391,10 @@ void GcodeSuite::M493() { // Parse zeta parameter (Y axis). if (parser.seenval('J')) { const float val = parser.value_float(); - if (ftMotion.cfg.modeHasShaper()) { + if (CMPNSTR_HAS_SHAPER(Y_AXIS)) { if (WITHIN(val, 0.01f, 1.0f)) { ftMotion.cfg.zeta[1] = val; - flag.update_n = flag.update_a = true; + flag.update = true; } else SERIAL_ECHOLNPGM("Invalid Y zeta [", C('J'), "] value."); // Zeta Out of range @@ -366,10 +406,10 @@ void GcodeSuite::M493() { // Parse vtol parameter (Y axis). if (parser.seenval('R')) { const float val = parser.value_float(); - if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) { + if (CMPNSTR_IS_EISHAPER(Y_AXIS)) { if (WITHIN(val, 0.00f, 1.0f)) { ftMotion.cfg.vtol[1] = val; - flag.update_a = true; + flag.update = true; } else SERIAL_ECHOLNPGM("Invalid Y vtol [", C('R'), "] value."); // VTol out of range. @@ -382,9 +422,7 @@ void GcodeSuite::M493() { planner.synchronize(); - if (flag.update_n) ftMotion.refreshShapingN(); - - if (flag.update_a) ftMotion.updateShapingA(); + if (flag.update) ftMotion.update_shaping_params(); if (flag.reset_ft) { stepper.ftMotion_syncPosition(); diff --git a/Marlin/src/inc/SanityCheck.h b/Marlin/src/inc/SanityCheck.h index 6b9deef84dc..e733f87e4e8 100644 --- a/Marlin/src/inc/SanityCheck.h +++ b/Marlin/src/inc/SanityCheck.h @@ -4350,6 +4350,15 @@ static_assert(_PLUS_TEST(3), "DEFAULT_MAX_ACCELERATION values must be positive." #elif DISABLED(FTM_UNIFIED_BWS) #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."); + #endif + #if HAS_DYNAMIC_FREQ_MM + static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis."); + #endif + #if HAS_DYNAMIC_FREQ_G + static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_MASS_BASED, "dynFreqMode_MASS_BASED requires an X axis and an extruder."); + #endif #endif // Multi-Stepping Limit diff --git a/Marlin/src/lcd/language/language_en.h b/Marlin/src/lcd/language/language_en.h index f30b29dd4e6..5d49a6afee5 100644 --- a/Marlin/src/lcd/language/language_en.h +++ b/Marlin/src/lcd/language/language_en.h @@ -814,7 +814,7 @@ namespace LanguageNarrow_en { LSTR MSG_BACKLASH_SMOOTHING = _UxGT("Smoothing"); LSTR MSG_FIXED_TIME_MOTION = _UxGT("Fixed-Time Motion"); - LSTR MSG_FTM_MODE = _UxGT("Motion Mode:"); + LSTR MSG_FTM_CMPN_MODE = _UxGT("@ Comp. Mode:"); LSTR MSG_FTM_ZV = _UxGT("ZV"); LSTR MSG_FTM_ZVD = _UxGT("ZVD"); LSTR MSG_FTM_ZVDD = _UxGT("ZVDD"); diff --git a/Marlin/src/lcd/menu/menu_motion.cpp b/Marlin/src/lcd/menu/menu_motion.cpp index f65eb96719a..40c2c14c0d4 100644 --- a/Marlin/src/lcd/menu/menu_motion.cpp +++ b/Marlin/src/lcd/menu/menu_motion.cpp @@ -325,39 +325,51 @@ void menu_move() { #include "../../module/ft_motion.h" #include "../../gcode/gcode.h" - void ftm_menu_setShaping(const ftMotionMode_t s) { - ftMotion.cfg.mode = s; - ftMotion.refreshShapingN(); + void ftm_menu_set_cmpn(const AxisEnum axis, const ftMotionShaper_t s) { + ftMotion.cfg.shaper[axis] = s; + ftMotion.update_shaping_params(); ui.go_back(); } - inline void menu_ftm_mode() { - const ftMotionMode_t mode = ftMotion.cfg.mode; - + inline void menu_ftm_cmpn_x() { + const ftMotionShaper_t shaper = ftMotion.cfg.shaper[X_AXIS]; START_MENU(); BACK_ITEM(MSG_FIXED_TIME_MOTION); - if (mode != ftMotionMode_DISABLED) ACTION_ITEM(MSG_LCD_OFF, []{ ftm_menu_setShaping(ftMotionMode_DISABLED); }); - if (mode != ftMotionMode_ENABLED) ACTION_ITEM(MSG_LCD_ON, []{ ftm_menu_setShaping(ftMotionMode_ENABLED); }); - #if HAS_X_AXIS - if (mode != ftMotionMode_ZV) ACTION_ITEM(MSG_FTM_ZV, []{ ftm_menu_setShaping(ftMotionMode_ZV); }); - if (mode != ftMotionMode_ZVD) ACTION_ITEM(MSG_FTM_ZVD, []{ ftm_menu_setShaping(ftMotionMode_ZVD); }); - if (mode != ftMotionMode_ZVDD) ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_setShaping(ftMotionMode_ZVDD); }); - if (mode != ftMotionMode_ZVDDD) ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_setShaping(ftMotionMode_ZVDDD); }); - if (mode != ftMotionMode_EI) ACTION_ITEM(MSG_FTM_EI, []{ ftm_menu_setShaping(ftMotionMode_EI); }); - if (mode != ftMotionMode_2HEI) ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_setShaping(ftMotionMode_2HEI); }); - if (mode != ftMotionMode_3HEI) ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_setShaping(ftMotionMode_3HEI); }); - if (mode != ftMotionMode_MZV) ACTION_ITEM(MSG_FTM_MZV, []{ ftm_menu_setShaping(ftMotionMode_MZV); }); - //if (mode != ftMotionMode_ULENDO_FBS) ACTION_ITEM(MSG_FTM_ULENDO_FBS, []{ ftm_menu_setShaping(ftMotionMode_ULENDO_FBS); }); - //if (mode != ftMotionMode_DISCTF) ACTION_ITEM(MSG_FTM_DISCTF, []{ ftm_menu_setShaping(ftMotionMode_DISCTF); }); - #endif + if (shaper != ftMotionShaper_NONE) ACTION_ITEM(MSG_LCD_OFF, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_NONE); }); + if (shaper != ftMotionShaper_ZV) ACTION_ITEM(MSG_FTM_ZV, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_ZV); }); + if (shaper != ftMotionShaper_ZVD) ACTION_ITEM(MSG_FTM_ZVD, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_ZVD); }); + if (shaper != ftMotionShaper_ZVDD) ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_ZVDD); }); + if (shaper != ftMotionShaper_ZVDDD) ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_ZVDDD); }); + if (shaper != ftMotionShaper_EI) ACTION_ITEM(MSG_FTM_EI, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_EI); }); + if (shaper != ftMotionShaper_2HEI) ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_2HEI); }); + if (shaper != ftMotionShaper_3HEI) ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_3HEI); }); + if (shaper != ftMotionShaper_MZV) ACTION_ITEM(MSG_FTM_MZV, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_MZV); }); + + END_MENU(); + } + + inline void menu_ftm_cmpn_y() { + const ftMotionShaper_t shaper = ftMotion.cfg.shaper[Y_AXIS]; + START_MENU(); + BACK_ITEM(MSG_FIXED_TIME_MOTION); + + if (shaper != ftMotionShaper_NONE) ACTION_ITEM(MSG_LCD_OFF, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_NONE); }); + if (shaper != ftMotionShaper_ZV) ACTION_ITEM(MSG_FTM_ZV, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_ZV); }); + if (shaper != ftMotionShaper_ZVD) ACTION_ITEM(MSG_FTM_ZVD, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_ZVD); }); + if (shaper != ftMotionShaper_ZVDD) ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_ZVDD); }); + if (shaper != ftMotionShaper_ZVDDD) ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_ZVDDD); }); + if (shaper != ftMotionShaper_EI) ACTION_ITEM(MSG_FTM_EI, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_EI); }); + if (shaper != ftMotionShaper_2HEI) ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_2HEI); }); + if (shaper != ftMotionShaper_3HEI) ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_3HEI); }); + if (shaper != ftMotionShaper_MZV) ACTION_ITEM(MSG_FTM_MZV, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_MZV); }); END_MENU(); } #if HAS_DYNAMIC_FREQ - inline void menu_ftm_dyn_mode() { + void menu_ftm_dyn_mode() { const dynFreqMode_t dmode = ftMotion.cfg.dynFreqMode; START_MENU(); @@ -379,22 +391,23 @@ void menu_move() { void menu_ft_motion() { ft_config_t &c = ftMotion.cfg; - FSTR_P ftmode; - switch (c.mode) { - default: - case ftMotionMode_DISABLED: ftmode = GET_TEXT_F(MSG_LCD_OFF); break; - case ftMotionMode_ENABLED: ftmode = GET_TEXT_F(MSG_LCD_ON); break; - case ftMotionMode_ZV: ftmode = GET_TEXT_F(MSG_FTM_ZV); break; - case ftMotionMode_ZVD: ftmode = GET_TEXT_F(MSG_FTM_ZVD); break; - case ftMotionMode_ZVDD: ftmode = GET_TEXT_F(MSG_FTM_ZVDD); break; - case ftMotionMode_ZVDDD: ftmode = GET_TEXT_F(MSG_FTM_ZVDDD);break; - case ftMotionMode_EI: ftmode = GET_TEXT_F(MSG_FTM_EI); break; - case ftMotionMode_2HEI: ftmode = GET_TEXT_F(MSG_FTM_2HEI); break; - case ftMotionMode_3HEI: ftmode = GET_TEXT_F(MSG_FTM_3HEI); break; - case ftMotionMode_MZV: ftmode = GET_TEXT_F(MSG_FTM_MZV); break; - //case ftMotionMode_ULENDO_FBS: ftmode = GET_TEXT_F(MSG_FTM_ULENDO_FBS); break; - //case ftMotionMode_DISCTF: ftmode = GET_TEXT_F(MSG_FTM_DISCTF); break; - } + FSTR_P ftshaper[1 + ENABLED(HAS_Y_AXIS)] {}; + + #if HAS_X_AXIS + for (uint_fast8_t a = X_AXIS; a <= TERN(HAS_Y_AXIS, Y_AXIS, X_AXIS); ++a) { + switch (c.shaper[a]) { + case ftMotionShaper_NONE: ftshaper[a] = GET_TEXT_F(MSG_LCD_OFF); break; + case ftMotionShaper_ZV: ftshaper[a] = GET_TEXT_F(MSG_FTM_ZV); break; + case ftMotionShaper_ZVD: ftshaper[a] = GET_TEXT_F(MSG_FTM_ZVD); break; + case ftMotionShaper_ZVDD: ftshaper[a] = GET_TEXT_F(MSG_FTM_ZVDD); break; + case ftMotionShaper_ZVDDD: ftshaper[a] = GET_TEXT_F(MSG_FTM_ZVDDD);break; + case ftMotionShaper_EI: ftshaper[a] = GET_TEXT_F(MSG_FTM_EI); break; + case ftMotionShaper_2HEI: ftshaper[a] = GET_TEXT_F(MSG_FTM_2HEI); break; + case ftMotionShaper_3HEI: ftshaper[a] = GET_TEXT_F(MSG_FTM_3HEI); break; + case ftMotionShaper_MZV: ftshaper[a] = GET_TEXT_F(MSG_FTM_MZV); break; + } + } + #endif #if HAS_DYNAMIC_FREQ FSTR_P dmode; @@ -409,32 +422,35 @@ void menu_move() { START_MENU(); BACK_ITEM(MSG_MOTION); - SUBMENU(MSG_FTM_MODE, menu_ftm_mode); - MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(ftmode); MENU_ITEM_ADDON_END(); + bool show_state = ftMotion.cfg.active; + EDIT_ITEM(bool, MSG_FIXED_TIME_MOTION, &show_state, []{ + ftMotion.cfg.active ^= true; + ftMotion.update_shaping_params(); + }); - if (c.modeHasShaper()) { + if (c.active) { #if HAS_X_AXIS - 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.refreshShapingN); + SUBMENU_N(X_AXIS, MSG_FTM_CMPN_MODE, menu_ftm_cmpn_x); + MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(ftshaper[X_AXIS]); MENU_ITEM_ADDON_END(); + + if (CMPNSTR_HAS_SHAPER(X_AXIS)) { + 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); + if (CMPNSTR_IS_EISHAPER(X_AXIS)) + EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol[0], 0.0f, 1.0f, ftMotion.update_shaping_params); + } #endif #if HAS_Y_AXIS - 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.refreshShapingN); - #endif + SUBMENU_N(Y_AXIS, MSG_FTM_CMPN_MODE, menu_ftm_cmpn_y); + MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(ftshaper[Y_AXIS]); MENU_ITEM_ADDON_END(); - #if HAS_X_AXIS - EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta[0], 0.0f, 1.0f, ftMotion.refreshShapingN); + if (CMPNSTR_HAS_SHAPER(Y_AXIS)) { + 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); + if (CMPNSTR_IS_EISHAPER(Y_AXIS)) + EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol[1], 0.0f, 1.0f, ftMotion.update_shaping_params); + } #endif - #if HAS_Y_AXIS - EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta[1], 0.0f, 1.0f, ftMotion.refreshShapingN); - #endif - - if (IS_EI_MODE(c.mode)) { - #if HAS_X_AXIS - EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol[0], 0.0f, 1.0f, ftMotion.refreshShapingN); - #endif - #if HAS_Y_AXIS - EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol[1], 0.0f, 1.0f, ftMotion.refreshShapingN); - #endif - } #if HAS_DYNAMIC_FREQ SUBMENU(MSG_FTM_DYN_MODE, menu_ftm_dyn_mode); @@ -448,13 +464,12 @@ void menu_move() { #endif } #endif + + #if HAS_EXTRUDERS + EDIT_ITEM(bool, MSG_LINEAR_ADVANCE, &c.linearAdvEna); + if (c.linearAdvEna) EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10); + #endif } - - #if HAS_EXTRUDERS - EDIT_ITEM(bool, MSG_LINEAR_ADVANCE, &c.linearAdvEna); - if (c.linearAdvEna) EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10); - #endif - END_MENU(); } diff --git a/Marlin/src/module/endstops.cpp b/Marlin/src/module/endstops.cpp index e9aafa49c84..03f5ab6d8e2 100644 --- a/Marlin/src/module/endstops.cpp +++ b/Marlin/src/module/endstops.cpp @@ -50,10 +50,6 @@ #include "../feature/joystick.h" #endif -#if ENABLED(FT_MOTION) - #include "ft_motion.h" -#endif - #if HAS_BED_PROBE #include "probe.h" #endif @@ -837,19 +833,9 @@ void Endstops::update() { #endif // Signal, after validation, if an endstop limit is pressed or not - bool moving_neg; - auto axis_moving_info = [](const AxisEnum axis, const AxisEnum head, bool &neg) -> bool { - #if ENABLED(FT_MOTION) - if (ftMotion.cfg.mode != ftMotionMode_DISABLED) - return (neg = ftMotion.axis_moving_neg(head)) || ftMotion.axis_moving_pos(head); - #endif - neg = !stepper.motor_direction(head); - return stepper.axis_is_moving(axis); - }; - #if HAS_X_AXIS - if (axis_moving_info(X_AXIS, X_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // -direction + if (stepper.axis_is_moving(X_AXIS)) { + if (!stepper.motor_direction(X_AXIS_HEAD)) { // -direction #if HAS_X_MIN_STATE PROCESS_ENDSTOP_X(MIN); #if CORE_DIAG(XY, Y, MIN) @@ -881,8 +867,8 @@ void Endstops::update() { #endif // HAS_X_AXIS #if HAS_Y_AXIS - if (axis_moving_info(Y_AXIS, Y_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // -direction + if (stepper.axis_is_moving(Y_AXIS)) { + if (!stepper.motor_direction(Y_AXIS_HEAD)) { // -direction #if HAS_Y_MIN_STATE PROCESS_ENDSTOP_Y(MIN); #if CORE_DIAG(XY, X, MIN) @@ -914,8 +900,8 @@ void Endstops::update() { #endif // HAS_Y_AXIS #if HAS_Z_AXIS - if (axis_moving_info(Z_AXIS, Z_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // Z -direction. Gantry down, bed up. + if (stepper.axis_is_moving(Z_AXIS)) { + if (!stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up. #if HAS_Z_MIN_STATE // If the Z_MIN_PIN is being used for the probe there's no // separate Z_MIN endstop. But a Z endstop could be wired @@ -959,8 +945,8 @@ void Endstops::update() { #endif // HAS_Z_AXIS #if HAS_I_AXIS - if (axis_moving_info(I_AXIS, I_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // -direction + if (stepper.axis_is_moving(I_AXIS)) { + if (!stepper.motor_direction(I_AXIS_HEAD)) { // -direction #if HAS_I_MIN_STATE PROCESS_ENDSTOP(I, MIN); #endif @@ -974,8 +960,8 @@ void Endstops::update() { #endif // HAS_I_AXIS #if HAS_J_AXIS - if (axis_moving_info(J_AXIS, J_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // -direction + if (stepper.axis_is_moving(J_AXIS)) { + if (!stepper.motor_direction(J_AXIS_HEAD)) { // -direction #if HAS_J_MIN_STATE PROCESS_ENDSTOP(J, MIN); #endif @@ -989,8 +975,8 @@ void Endstops::update() { #endif // HAS_J_AXIS #if HAS_K_AXIS - if (axis_moving_info(K_AXIS, K_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // -direction + if (stepper.axis_is_moving(K_AXIS)) { + if (!stepper.motor_direction(K_AXIS_HEAD)) { // -direction #if HAS_K_MIN_STATE PROCESS_ENDSTOP(K, MIN); #endif @@ -1004,8 +990,8 @@ void Endstops::update() { #endif // HAS_K_AXIS #if HAS_U_AXIS - if (axis_moving_info(U_AXIS, U_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // -direction + if (stepper.axis_is_moving(U_AXIS)) { + if (!stepper.motor_direction(U_AXIS_HEAD)) { // -direction #if HAS_U_MIN_STATE PROCESS_ENDSTOP(U, MIN); #endif @@ -1019,8 +1005,8 @@ void Endstops::update() { #endif // HAS_U_AXIS #if HAS_V_AXIS - if (axis_moving_info(V_AXIS, V_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // -direction + if (stepper.axis_is_moving(V_AXIS)) { + if (!stepper.motor_direction(V_AXIS_HEAD)) { // -direction #if HAS_V_MIN_STATE PROCESS_ENDSTOP(V, MIN); #endif @@ -1034,8 +1020,8 @@ void Endstops::update() { #endif // HAS_V_AXIS #if HAS_W_AXIS - if (axis_moving_info(W_AXIS, W_AXIS_HEAD, moving_neg)) { - if (moving_neg) { // -direction + if (stepper.axis_is_moving(W_AXIS)) { + if (!stepper.motor_direction(W_AXIS_HEAD)) { // -direction #if HAS_W_MIN_STATE PROCESS_ENDSTOP(W, MIN); #endif diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index e73c75b9c51..0099b3a25ee 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -30,23 +30,6 @@ FTMotion ftMotion; -#if !HAS_X_AXIS - static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZV, "ftMotionMode_ZV requires at least one linear axis."); - static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVD, "ftMotionMode_ZVD requires at least one linear axis."); - static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVDD, "ftMotionMode_ZVD requires at least one linear axis."); - static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVDDD, "ftMotionMode_ZVD requires at least one linear axis."); - static_assert(FTM_DEFAULT_MODE == ftMotionMode_EI, "ftMotionMode_EI requires at least one linear axis."); - static_assert(FTM_DEFAULT_MODE == ftMotionMode_2HEI, "ftMotionMode_2HEI requires at least one linear axis."); - static_assert(FTM_DEFAULT_MODE == ftMotionMode_3HEI, "ftMotionMode_3HEI requires at least one linear axis."); - static_assert(FTM_DEFAULT_MODE == ftMotionMode_MZV, "ftMotionMode_MZV requires at least one linear axis."); -#endif -#if !HAS_DYNAMIC_FREQ_MM - static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis."); -#endif -#if !HAS_DYNAMIC_FREQ_G - static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_MASS_BASED, "dynFreqMode_MASS_BASED requires an X axis and an extruder."); -#endif - //----------------------------------------------------------------- // Variables. //----------------------------------------------------------------- @@ -60,8 +43,6 @@ int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper comman FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer. bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use. -millis_t FTMotion::axis_pos_move_end_ti[NUM_AXIS_ENUMS] = {0}, - FTMotion::axis_neg_move_end_ti[NUM_AXIS_ENUMS] = {0}; // Private variables. @@ -70,17 +51,15 @@ millis_t FTMotion::axis_pos_move_end_ti[NUM_AXIS_ENUMS] = {0}, xyze_trajectory_t FTMotion::traj; // = {0.0f} Storage for fixed-time-based trajectory. xyze_trajectoryMod_t FTMotion::trajMod; // = {0.0f} Storage for fixed time trajectory window. -bool FTMotion::blockProcRdy = false, // Indicates a block is ready to be processed. - FTMotion::blockProcRdy_z1 = false, // Storage for the previous indicator. - FTMotion::blockProcDn = false; // Indicates current block is done being processed. -bool FTMotion::batchRdy = false; // Indicates a batch of the fixed time trajectory - // has been generated, is now available in the upper - - // half of traj.x[], y, z ... e vectors, and is ready to be - // post processed, if applicable, then interpolated. -bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done being post processed, - // if applicable, and is ready to be converted to step commands. -bool FTMotion::runoutEna = false; // True if runout of the block hasn't been done and is allowed. -bool FTMotion::blockDataIsRunout = false; // Indicates the last loaded block variables are for a runout. +bool FTMotion::blockProcRdy = false; // Set when new block data is loaded from stepper module into FTM, ... + // ... and reset when block is completely converted to FTM trajectory. +bool FTMotion::batchRdy = false; // Indicates a batch of the fixed time trajectory... + // ... has been generated, is now available in the upper - + // batch of traj.x[], y, z ... e vectors, and is ready to be + // post processed, if applicable, then interpolated. Reset when the + // data has been shifted out. +bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done being post processed... + // ... if applicable, and is ready to be converted to step commands. // Trapezoid data variables. xyze_pos_t FTMotion::startPosn, // (mm) Start position of block @@ -101,22 +80,20 @@ uint32_t FTMotion::max_intervals; // Total number of data points t // Make vector variables. uint32_t FTMotion::makeVector_idx = 0, // Index of fixed time trajectory generation of the overall block. - FTMotion::makeVector_idx_z1 = 0, // Storage for the previously calculated index above. FTMotion::makeVector_batchIdx = 0; // Index of fixed time trajectory generation within the batch. // Interpolation variables. xyze_long_t FTMotion::steps = { 0 }; // Step count accumulator. -uint32_t FTMotion::interpIdx = 0, // Index of current data point being interpolated. - FTMotion::interpIdx_z1 = 0; // Storage for the previously calculated index above. +uint32_t FTMotion::interpIdx = 0; // Index of current data point being interpolated. // Shaping variables. #if HAS_X_AXIS FTMotion::shaping_t FTMotion::shaping = { - 0, 0, - x:{ false, { 0.0f }, { 0.0f }, { 0 } }, // d_zi, Ai, Ni + 0, + x:{ false, { 0.0f }, { 0.0f }, { 0 }, { 0 } }, // ena, d_zi, Ai, Ni, max_i #if HAS_Y_AXIS - y:{ false, { 0.0f }, { 0.0f }, { 0 } } // d_zi, Ai, Ni + y:{ false, { 0.0f }, { 0.0f }, { 0 }, { 0 } } // ena, d_zi, Ai, Ni, max_i #endif }; #endif @@ -127,7 +104,7 @@ uint32_t FTMotion::interpIdx = 0, // Index of current data point b float FTMotion::e_advanced_z1 = 0.0f; // (ms) Unit delay of advanced extruder position. #endif -constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE); +constexpr uint32_t BATCH_SIDX_IN_WINDOW = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE); // Batch start index in window. //----------------------------------------------------------------- // Function definitions. @@ -137,41 +114,10 @@ constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE); static bool markBlockStart = false; -// Sets controller states to begin processing a block. -// Called by Stepper::ftMotion_blockQueueUpdate, invoked from the main loop. -void FTMotion::startBlockProc() { - blockProcRdy = true; - blockProcDn = false; - runoutEna = true; -} - -// Move any free data points to the stepper buffer even if a full batch isn't ready. -void FTMotion::runoutBlock() { - - if (!runoutEna) return; - - startPosn = endPosn_prevBlock; - ratio.reset(); - - max_intervals = cfg.modeHasShaper() ? shaper_intervals : 0; - if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BATCH_SIZE, min_max_intervals - (FTM_BATCH_SIZE))) - max_intervals = min_max_intervals; - - max_intervals += ( - #if ENABLED(FTM_UNIFIED_BWS) - FTM_WINDOW_SIZE - makeVector_batchIdx - #else - FTM_WINDOW_SIZE - ((last_batchIdx < (FTM_BATCH_SIZE)) ? 0 : makeVector_batchIdx) - #endif - ); - blockProcRdy = blockDataIsRunout = true; - runoutEna = blockProcDn = false; -} - // Controller main, to be invoked from non-isr task. void FTMotion::loop() { - if (!cfg.mode) return; + if (!cfg.active) return; /** * Handle block abort with the following sequence: @@ -182,24 +128,49 @@ void FTMotion::loop() { */ if (stepper.abort_current_block) { if (sts_stepperBusy) return; // Wait until motion buffers are emptied + discard_planner_block_protected(); reset(); - blockProcDn = true; // Set queueing to look for next block. stepper.abort_current_block = false; // Abort finished. } - // Planner processing and block conversion. - if (!blockProcRdy) stepper.ftMotion_blockQueueUpdate(); + while (!blockProcRdy && (stepper.current_block = planner.get_current_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(); + continue; + } + loadBlockData(stepper.current_block); + markBlockStart = true; + blockProcRdy = true; + // Some kinematics track axis motion in HX, HY, HZ + #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX) + stepper.last_direction_bits.hx = stepper.current_block->direction_bits.hx; + #endif + #if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX) + stepper.last_direction_bits.hy = stepper.current_block->direction_bits.hy; + #endif + #if ANY(CORE_IS_XZ, CORE_IS_YZ) + stepper.last_direction_bits.hz = stepper.current_block->direction_bits.hz; + #endif + } if (blockProcRdy) { - if (!blockProcRdy_z1) { // One-shot. - if (!blockDataIsRunout) { - loadBlockData(stepper.current_block); - markBlockStart = true; + + if (!batchRdy) makeVector(); // Caution: Do not consolidate checks on blockProcRdy/batchRdy, as they are written by makeVector(). + // When makeVector is finished: either blockProcRdy has been set false (because the block is + // done being processed) or batchRdy is set true, or both. + + // Check if the block has been completely converted: + if (!blockProcRdy) { + discard_planner_block_protected(); + + // Check if the block needs to be runout: + if (!batchRdy && !planner.movesplanned()){ + runoutBlock(); + makeVector(); // Do an additional makeVector call to guarantee batchRdy set this loop. } - else blockDataIsRunout = false; } - while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP))) - makeVector(); } // FBS / post processing. @@ -215,7 +186,7 @@ void FTMotion::loop() { LOGICAL_AXIS_MAP_LC(TCOPY); // Shift the time series back in the window - #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], last_batchIdx * sizeof(traj.A[0])); + #define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], BATCH_SIDX_IN_WINDOW * sizeof(traj.A[0])); LOGICAL_AXIS_MAP_LC(TSHIFT); #endif @@ -227,9 +198,7 @@ void FTMotion::loop() { // Interpolation. while (batchRdyForInterp - && (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME)) - && (interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP)) - ) { + && (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))) { convertToSteps(interpIdx); if (++interpIdx == FTM_BATCH_SIZE) { batchRdyForInterp = false; @@ -238,196 +207,139 @@ void FTMotion::loop() { } // Report busy status to planner. - busy = (sts_stepperBusy || ((!blockProcDn && blockProcRdy) || batchRdy || batchRdyForInterp || runoutEna)); + busy = (sts_stepperBusy || blockProcRdy || batchRdy || batchRdyForInterp); - blockProcRdy_z1 = blockProcRdy; - makeVector_idx_z1 = makeVector_idx; - interpIdx_z1 = interpIdx; } #if HAS_X_AXIS // Refresh the gains used by shaping functions. - // To be called on init or mode or zeta change. + void FTMotion::AxisShaping::set_axis_shaping_A(const ftMotionShaper_t shaper, const_float_t zeta, const_float_t vtol) { - void FTMotion::Shaping::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) { + const float K = exp(-zeta * M_PI / sqrt(1.f - sq(zeta))), + K2 = sq(K), + K3 = K2 * K, + K4 = K3 * K; - const float Kx = exp(-zeta[0] * M_PI / sqrt(1.0f - sq(zeta[0]))), - Ky = exp(-zeta[1] * M_PI / sqrt(1.0f - sq(zeta[1]))), - Kx2 = sq(Kx), - Ky2 = sq(Ky); + switch (shaper) { - switch (cfg.mode) { - - case ftMotionMode_ZV: + case ftMotionShaper_ZV: max_i = 1U; - x.Ai[0] = 1.0f / (1.0f + Kx); - x.Ai[1] = x.Ai[0] * Kx; - - y.Ai[0] = 1.0f / (1.0f + Ky); - y.Ai[1] = y.Ai[0] * Ky; + Ai[0] = 1.0f / (1.0f + K); + Ai[1] = Ai[0] * K; break; - case ftMotionMode_ZVD: + case ftMotionShaper_ZVD: max_i = 2U; - x.Ai[0] = 1.0f / (1.0f + 2.0f * Kx + Kx2); - x.Ai[1] = x.Ai[0] * 2.0f * Kx; - x.Ai[2] = x.Ai[0] * Kx2; - - y.Ai[0] = 1.0f / (1.0f + 2.0f * Ky + Ky2); - y.Ai[1] = y.Ai[0] * 2.0f * Ky; - y.Ai[2] = y.Ai[0] * Ky2; + Ai[0] = 1.0f / (1.0f + 2.0f * K + K2); + Ai[1] = Ai[0] * 2.0f * K; + Ai[2] = Ai[0] * K2; break; - case ftMotionMode_ZVDD: + case ftMotionShaper_ZVDD: max_i = 3U; - x.Ai[0] = 1.0f / (1.0f + 3.0f * Kx + 3.0f * Kx2 + cu(Kx)); - x.Ai[1] = x.Ai[0] * 3.0f * Kx; - x.Ai[2] = x.Ai[0] * 3.0f * Kx2; - x.Ai[3] = x.Ai[0] * cu(Kx); - - y.Ai[0] = 1.0f / (1.0f + 3.0f * Ky + 3.0f * Ky2 + cu(Ky)); - y.Ai[1] = y.Ai[0] * 3.0f * Ky; - y.Ai[2] = y.Ai[0] * 3.0f * Ky2; - y.Ai[3] = y.Ai[0] * cu(Ky); + Ai[0] = 1.0f / (1.0f + 3.0f * K + 3.0f * K2 + K3); + Ai[1] = Ai[0] * 3.0f * K; + Ai[2] = Ai[0] * 3.0f * K2; + Ai[3] = Ai[0] * K3; break; - case ftMotionMode_ZVDDD: + case ftMotionShaper_ZVDDD: max_i = 4U; - x.Ai[0] = 1.0f / (1.0f + 4.0f * Kx + 6.0f * Kx2 + 4.0f * cu(Kx) + sq(Kx2)); - x.Ai[1] = x.Ai[0] * 4.0f * Kx; - x.Ai[2] = x.Ai[0] * 6.0f * Kx2; - x.Ai[3] = x.Ai[0] * 4.0f * cu(Kx); - x.Ai[4] = x.Ai[0] * sq(Kx2); - - y.Ai[0] = 1.0f / (1.0f + 4.0f * Ky + 6.0f * Ky2 + 4.0f * cu(Ky) + sq(Ky2)); - y.Ai[1] = y.Ai[0] * 4.0f * Ky; - y.Ai[2] = y.Ai[0] * 6.0f * Ky2; - y.Ai[3] = y.Ai[0] * 4.0f * cu(Ky); - y.Ai[4] = y.Ai[0] * sq(Ky2); + Ai[0] = 1.0f / (1.0f + 4.0f * K + 6.0f * K2 + 4.0f * K3 + K4); + Ai[1] = Ai[0] * 4.0f * K; + Ai[2] = Ai[0] * 6.0f * K2; + Ai[3] = Ai[0] * 4.0f * K3; + Ai[4] = Ai[0] * K4; break; - case ftMotionMode_EI: { + case ftMotionShaper_EI: { max_i = 2U; - x.Ai[0] = 0.25f * (1.0f + vtol[0]); - x.Ai[1] = 0.50f * (1.0f - vtol[0]) * Kx; - x.Ai[2] = x.Ai[0] * Kx2; + Ai[0] = 0.25f * (1.0f + vtol); + Ai[1] = 0.50f * (1.0f - vtol) * K; + Ai[2] = Ai[0] * K2; - y.Ai[0] = 0.25f * (1.0f + vtol[1]); - y.Ai[1] = 0.50f * (1.0f - vtol[1]) * Ky; - y.Ai[2] = y.Ai[0] * Ky2; - - const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2]); - const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2]); + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2]); for (uint32_t i = 0U; i < 3U; i++) { - x.Ai[i] *= X_adj; - y.Ai[i] *= Y_adj; + Ai[i] *= adj; } } break; - case ftMotionMode_2HEI: { + case ftMotionShaper_2HEI: { max_i = 3U; - const float vtolx2 = sq(vtol[0]); + const float vtolx2 = sq(vtol); const float X = pow(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f); - x.Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X); - x.Ai[1] = (0.5f - x.Ai[0]) * Kx; - x.Ai[2] = x.Ai[1] * Kx; - x.Ai[3] = x.Ai[0] * cu(Kx); + Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X); + Ai[1] = (0.5f - Ai[0]) * K; + Ai[2] = Ai[1] * K; + Ai[3] = Ai[0] * K3; - const float vtoly2 = sq(vtol[1]); - const float Y = pow(vtoly2 * (sqrt(1.0f - vtoly2) + 1.0f), 1.0f / 3.0f); - y.Ai[0] = (3.0f * sq(Y) + 2.0f * Y + 3.0f * vtoly2) / (16.0f * Y); - y.Ai[1] = (0.5f - y.Ai[0]) * Ky; - y.Ai[2] = y.Ai[1] * Ky; - y.Ai[3] = y.Ai[0] * cu(Ky); - - const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3]); - const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2] + y.Ai[3]); + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3]); for (uint32_t i = 0U; i < 4U; i++) { - x.Ai[i] *= X_adj; - y.Ai[i] *= Y_adj; + Ai[i] *= adj; } } break; - case ftMotionMode_3HEI: { + case ftMotionShaper_3HEI: { max_i = 4U; - x.Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol[0] + 2.0f * sqrt( 2.0f * ( vtol[0] + 1.0f ) * vtol[0] ) ); - x.Ai[1] = 0.25f * ( 1.0f - vtol[0] ) * Kx; - x.Ai[2] = ( 0.5f * ( 1.0f + vtol[0] ) - 2.0f * x.Ai[0] ) * Kx2; - x.Ai[3] = x.Ai[1] * Kx2; - x.Ai[4] = x.Ai[0] * sq(Kx2); + Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) ); + Ai[1] = 0.25f * ( 1.0f - vtol ) * K; + Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * Ai[0] ) * K2; + Ai[3] = Ai[1] * K2; + Ai[4] = Ai[0] * K4; - y.Ai[0] = 0.0625f * (1.0f + 3.0f * vtol[1] + 2.0f * sqrt(2.0f * (vtol[1] + 1.0f) * vtol[1])); - y.Ai[1] = 0.25f * (1.0f - vtol[1]) * Ky; - y.Ai[2] = (0.5f * (1.0f + vtol[1]) - 2.0f * y.Ai[0]) * Ky2; - y.Ai[3] = y.Ai[1] * Ky2; - y.Ai[4] = y.Ai[0] * sq(Ky2); - - const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3] + x.Ai[4]); - const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2] + y.Ai[3] + y.Ai[4]); + const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3] + Ai[4]); for (uint32_t i = 0U; i < 5U; i++) { - x.Ai[i] *= X_adj; - y.Ai[i] *= Y_adj; + Ai[i] *= adj; } } break; - case ftMotionMode_MZV: { + case ftMotionShaper_MZV: { max_i = 2U; - const float Bx = 1.4142135623730950488016887242097f * Kx; - x.Ai[0] = 1.0f / (1.0f + Bx + Kx2); - x.Ai[1] = x.Ai[0] * Bx; - x.Ai[2] = x.Ai[0] * Kx2; - - const float By = 1.4142135623730950488016887242097f * Ky; - y.Ai[0] = 1.0f / (1.0f + By + Ky2); - y.Ai[1] = y.Ai[0] * By; - y.Ai[2] = y.Ai[0] * Ky2; + const float Bx = 1.4142135623730950488016887242097f * K; + Ai[0] = 1.0f / (1.0f + Bx + K2); + Ai[1] = Ai[0] * Bx; + Ai[2] = Ai[0] * K2; } break; default: - ZERO(x.Ai); - ZERO(y.Ai); + ZERO(Ai); max_i = 0; } } - void FTMotion::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) { - shaping.updateShapingA(zeta, vtol); - } - // Refresh the indices used by shaping functions. - // To be called when frequencies change. - - void FTMotion::AxisShaping::updateShapingN(const_float_t f, const_float_t df) { - // Protections omitted for DBZ and for index exceeding array length. - switch (cfg.mode) { - case ftMotionMode_ZV: + void FTMotion::AxisShaping::set_axis_shaping_N(const ftMotionShaper_t shaper, const_float_t f, const_float_t zeta) { + // Note that protections are omitted for DBZ and for index exceeding array length. + const float df = sqrt ( 1.f - sq(zeta) ); + switch (shaper) { + case ftMotionShaper_ZV: Ni[1] = round((0.5f / f / df) * (FTM_FS)); break; - case ftMotionMode_ZVD: - case ftMotionMode_EI: + case ftMotionShaper_ZVD: + case ftMotionShaper_EI: Ni[1] = round((0.5f / f / df) * (FTM_FS)); Ni[2] = Ni[1] + Ni[1]; break; - case ftMotionMode_ZVDD: - case ftMotionMode_2HEI: + case ftMotionShaper_ZVDD: + case ftMotionShaper_2HEI: Ni[1] = round((0.5f / f / df) * (FTM_FS)); Ni[2] = Ni[1] + Ni[1]; Ni[3] = Ni[2] + Ni[1]; break; - case ftMotionMode_ZVDDD: - case ftMotionMode_3HEI: + case ftMotionShaper_ZVDDD: + case ftMotionShaper_3HEI: Ni[1] = round((0.5f / f / df) * (FTM_FS)); Ni[2] = Ni[1] + Ni[1]; Ni[3] = Ni[2] + Ni[1]; Ni[4] = Ni[3] + Ni[1]; break; - case ftMotionMode_MZV: + case ftMotionShaper_MZV: Ni[1] = round((0.375f / f / df) * (FTM_FS)); Ni[2] = Ni[1] + Ni[1]; break; @@ -435,13 +347,20 @@ void FTMotion::loop() { } } - void FTMotion::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), float zeta[]/*=cfg.zeta*/) { - const float xdf = sqrt(1.0f - sq(zeta[0])); - shaping.x.updateShapingN(xf, xdf); - + void FTMotion::update_shaping_params() { + #if HAS_X_AXIS + shaping.x.ena = CMPNSTR_HAS_SHAPER(X_AXIS); + if (shaping.x.ena) { + 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]); + } + #endif #if HAS_Y_AXIS - const float ydf = sqrt(1.0f - sq(zeta[1])); - shaping.y.updateShapingN(yf, ydf); + shaping.y.ena = CMPNSTR_HAS_SHAPER(Y_AXIS); + if (shaping.y.ena) { + 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]); + } #endif } @@ -454,17 +373,17 @@ void FTMotion::reset() { traj.reset(); - blockProcRdy = blockProcRdy_z1 = blockProcDn = false; - batchRdy = batchRdyForInterp = false; - runoutEna = false; + blockProcRdy = batchRdy = batchRdyForInterp = false; endPosn_prevBlock.reset(); - makeVector_idx = makeVector_idx_z1 = 0; - makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MAX(last_batchIdx, FTM_BATCH_SIZE)); + makeVector_idx = 0; + makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MIN(BATCH_SIDX_IN_WINDOW, FTM_BATCH_SIZE)); steps.reset(); - interpIdx = interpIdx_z1 = 0; + interpIdx = 0; + + stepper.axis_did_move.reset(); #if HAS_X_AXIS ZERO(shaping.x.d_zi); @@ -473,13 +392,48 @@ void FTMotion::reset() { #endif TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f); - - ZERO(axis_pos_move_end_ti); - ZERO(axis_neg_move_end_ti); } // Private functions. +void FTMotion::discard_planner_block_protected() { + if (stepper.current_block) { // Safeguard in case current_block must not be null (it will + // be null when the "block" is a runout or generated) in order + // to use planner.release_current_block(). + stepper.current_block = nullptr; + planner.release_current_block(); // FTM uses release_current_block() instead of discard_current_block(), + // as in block_phase_isr(). This change is to avoid invoking axis_did_move.reset(). + // current_block = nullptr is added to replicate discard without axis_did_move reset. + // Note invoking axis_did_move.reset() causes no issue since FTM's stepper refreshes + // its values every ISR. + } +} + +// 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. +void FTMotion::runoutBlock() { + + startPosn = endPosn_prevBlock; + ratio.reset(); + + 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; + + 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; + + int32_t n_to_settle_and_fill_batch = n_to_settle_cmpnstr + n_to_fill_batch_after_settling; + + max_intervals = PROP_BATCHES * FTM_BATCH_SIZE + n_to_settle_and_fill_batch; + + blockProcRdy = true; +} + // Auxiliary function to get number of step commands in the buffer. int32_t FTMotion::stepperCmdBuffItems() { const int32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx; @@ -488,10 +442,7 @@ int32_t FTMotion::stepperCmdBuffItems() { // Initializes storage variables before startup. void FTMotion::init() { - #if HAS_X_AXIS - refreshShapingN(); - updateShapingA(); - #endif + update_shaping_params(); reset(); // Precautionary. } @@ -598,42 +549,6 @@ void FTMotion::loadBlockData(block_t * const current_block) { endPosn_prevBlock += moveDist; - millis_t move_end_ti = millis() + SEC_TO_MS(FTM_TS*(float)(max_intervals + num_samples_cmpnstr_settle() + (PROP_BATCHES+1)*FTM_BATCH_SIZE) + ((float)FTM_STEPPERCMD_BUFF_SIZE/(float)FTM_STEPPER_FS)); - - #if CORE_IS_XY - if (moveDist.x > 0.f) axis_pos_move_end_ti[A_AXIS] = move_end_ti; - if (moveDist.y > 0.f) axis_pos_move_end_ti[B_AXIS] = move_end_ti; - if (moveDist.x + moveDist.y > 0.f) axis_pos_move_end_ti[X_HEAD] = move_end_ti; - if (moveDist.x - moveDist.y > 0.f) axis_pos_move_end_ti[Y_HEAD] = move_end_ti; - if (moveDist.x < 0.f) axis_neg_move_end_ti[A_AXIS] = move_end_ti; - if (moveDist.y < 0.f) axis_neg_move_end_ti[B_AXIS] = move_end_ti; - if (moveDist.x + moveDist.y < 0.f) axis_neg_move_end_ti[X_HEAD] = move_end_ti; - if (moveDist.x - moveDist.y < 0.f) axis_neg_move_end_ti[Y_HEAD] = move_end_ti; - #else - if (moveDist.x > 0.f) axis_pos_move_end_ti[X_AXIS] = move_end_ti; - if (moveDist.y > 0.f) axis_pos_move_end_ti[Y_AXIS] = move_end_ti; - if (moveDist.x < 0.f) axis_neg_move_end_ti[X_AXIS] = move_end_ti; - if (moveDist.y < 0.f) axis_neg_move_end_ti[Y_AXIS] = move_end_ti; - #endif - if (moveDist.z > 0.f) axis_pos_move_end_ti[Z_AXIS] = move_end_ti; - if (moveDist.z < 0.f) axis_neg_move_end_ti[Z_AXIS] = move_end_ti; - // if (moveDist.i > 0.f) axis_pos_move_end_ti[I_AXIS] = move_end_ti; - // if (moveDist.i < 0.f) axis_neg_move_end_ti[I_AXIS] = move_end_ti; - // if (moveDist.j > 0.f) axis_pos_move_end_ti[J_AXIS] = move_end_ti; - // if (moveDist.j < 0.f) axis_neg_move_end_ti[J_AXIS] = move_end_ti; - // if (moveDist.k > 0.f) axis_pos_move_end_ti[K_AXIS] = move_end_ti; - // if (moveDist.k < 0.f) axis_neg_move_end_ti[K_AXIS] = move_end_ti; - // if (moveDist.u > 0.f) axis_pos_move_end_ti[U_AXIS] = move_end_ti; - // if (moveDist.u < 0.f) axis_neg_move_end_ti[U_AXIS] = move_end_ti; - // . - // . - // . - - // If the endstop is already pressed, endstop interrupts won't invoke - // endstop_triggered and the move will grind. So check here for a - // triggered endstop, which shortly marks the block for discard. - endstops.update(); - } // Generate data points of the trajectory. @@ -679,9 +594,14 @@ void FTMotion::makeVector() { #if HAS_DYNAMIC_FREQ_MM case dynFreqMode_Z_BASED: if (traj.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed. - const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx] - OPTARG(HAS_Y_AXIS, yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx]); - updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ) OPTARG(HAS_Y_AXIS, _MAX(yf, FTM_MIN_SHAPE_FREQ))); + #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]); + #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]); + #endif } break; #endif @@ -690,43 +610,49 @@ void FTMotion::makeVector() { case dynFreqMode_MASS_BASED: // Update constantly. The optimization done for Z value makes // less sense for E, as E is expected to constantly change. - updateShapingN( cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx] - OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx]) ); + #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]); + #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]); + #endif break; #endif default: break; } - // Apply shaping if in mode. + // Apply shaping if active on each axis #if HAS_X_AXIS - if (cfg.modeHasShaper()) { - shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx]; - traj.x[makeVector_batchIdx] *= shaping.x.Ai[0]; - #if HAS_Y_AXIS - shaping.y.d_zi[shaping.zi_idx] = traj.y[makeVector_batchIdx]; - traj.y[makeVector_batchIdx] *= shaping.y.Ai[0]; - #endif - for (uint32_t i = 1U; i <= shaping.max_i; i++) { - const uint32_t udiffx = shaping.zi_idx - shaping.x.Ni[i]; - traj.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx]; - #if HAS_Y_AXIS - const uint32_t udiffy = shaping.zi_idx - shaping.y.Ni[i]; - traj.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy]; - #endif + if (shaping.x.ena) { + shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx]; + traj.x[makeVector_batchIdx] *= shaping.x.Ai[0]; + for (uint32_t i = 1U; i <= shaping.x.max_i; i++) { + const uint32_t udiffx = shaping.zi_idx - shaping.x.Ni[i]; + traj.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx]; + } } + + #if HAS_Y_AXIS + if (shaping.y.ena) { + shaping.y.d_zi[shaping.zi_idx] = traj.y[makeVector_batchIdx]; + traj.y[makeVector_batchIdx] *= shaping.y.Ai[0]; + for (uint32_t i = 1U; i <= shaping.y.max_i; i++) { + const uint32_t udiffy = shaping.zi_idx - shaping.y.Ni[i]; + traj.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy]; + } + } + #endif if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0; - } - #endif + #endif // Filled up the queue with regular and shaped steps if (++makeVector_batchIdx == FTM_WINDOW_SIZE) { - makeVector_batchIdx = last_batchIdx; + makeVector_batchIdx = BATCH_SIDX_IN_WINDOW; batchRdy = true; } if (++makeVector_idx == max_intervals) { - blockProcDn = true; blockProcRdy = false; makeVector_idx = 0; } diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index 3325005ae28..4851a8cc7db 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -37,20 +37,20 @@ #endif typedef struct FTConfig { - ftMotionMode_t mode = FTM_DEFAULT_MODE; // Mode / active compensation mode configuration. - - bool modeHasShaper() { return WITHIN(mode, 10U, 19U); } + bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else standard motion) #if HAS_X_AXIS + ftMotionShaper_t shaper[1 + ENABLED(HAS_Y_AXIS)] = // Shaper type + { FTM_DEFAULT_SHAPER_X OPTARG(HAS_Y_AXIS, FTM_DEFAULT_SHAPER_Y) }; float baseFreq[1 + ENABLED(HAS_Y_AXIS)] = // Base frequency. [Hz] { FTM_SHAPING_DEFAULT_X_FREQ OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) }; float zeta[1 + ENABLED(HAS_Y_AXIS)] = // Damping factor - { FTM_SHAPING_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_ZETA_Y) }; + { FTM_SHAPING_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_ZETA_Y) }; float vtol[1 + ENABLED(HAS_Y_AXIS)] = // Vibration Level - { FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) }; + { FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) }; #endif -#if HAS_DYNAMIC_FREQ + #if HAS_DYNAMIC_FREQ dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration. float dynFreqK[1 + ENABLED(HAS_Y_AXIS)] = { 0.0f }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g] #else @@ -72,16 +72,21 @@ class FTMotion { static bool busy; static void set_defaults() { - cfg.mode = FTM_DEFAULT_MODE; + cfg.active = ENABLED(FTM_IS_DEFAULT_MOTION); - TERN_(HAS_X_AXIS, cfg.baseFreq[X_AXIS] = FTM_SHAPING_DEFAULT_X_FREQ); - TERN_(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] = FTM_SHAPING_DEFAULT_Y_FREQ); + #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; + #endif - TERN_(HAS_X_AXIS, cfg.zeta[X_AXIS] = FTM_SHAPING_ZETA_X); - TERN_(HAS_Y_AXIS, cfg.zeta[Y_AXIS] = FTM_SHAPING_ZETA_Y); - - TERN_(HAS_X_AXIS, cfg.vtol[X_AXIS] = FTM_SHAPING_V_TOL_X); - TERN_(HAS_Y_AXIS, cfg.vtol[Y_AXIS] = FTM_SHAPING_V_TOL_Y); + #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; + #endif #if HAS_DYNAMIC_FREQ cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; @@ -93,10 +98,7 @@ class FTMotion { cfg.linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; #endif - #if HAS_X_AXIS - refreshShapingN(); - updateShapingA(); - #endif + TERN_(HAS_X_AXIS, update_shaping_params()); reset(); } @@ -107,43 +109,24 @@ class FTMotion { static bool sts_stepperBusy; // The stepper buffer has items and is in use. - static millis_t axis_pos_move_end_ti[NUM_AXIS_ENUMS], - axis_neg_move_end_ti[NUM_AXIS_ENUMS]; - // Public methods static void init(); - static void startBlockProc(); // Set controller states to begin processing a block. - static bool getBlockProcDn() { return blockProcDn; } // Return true if the controller no longer needs the current block. - static void runoutBlock(); // Move any free data points to the stepper buffer even if a full batch isn't ready. static void loop(); // Controller main, to be invoked from non-isr task. #if HAS_X_AXIS - // Refresh the gains used by shaping functions. - // To be called on init or mode or zeta change. - static void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol); - - // Refresh the indices used by shaping functions. - // To be called when frequencies change. - static void updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), float zeta[]=cfg.zeta); - - static void refreshShapingN() { updateShapingN(cfg.baseFreq[X_AXIS] OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS])); } - + // Refresh gains and indices used by shaping functions. + static void update_shaping_params(void); #endif static void reset(); // Reset all states of the fixed time conversion to defaults. - static bool axis_moving_pos(const AxisEnum axis) { return !ELAPSED(millis(), axis_pos_move_end_ti[axis]); } - static bool axis_moving_neg(const AxisEnum axis) { return !ELAPSED(millis(), axis_neg_move_end_ti[axis]); } - private: static xyze_trajectory_t traj; static xyze_trajectoryMod_t trajMod; - static bool blockProcRdy, blockProcRdy_z1, blockProcDn; + static bool blockProcRdy; static bool batchRdy, batchRdyForInterp; - static bool runoutEna; - static bool blockDataIsRunout; // Trapezoid data variables. static xyze_pos_t startPosn, // (mm) Start position of block @@ -160,19 +143,12 @@ class FTMotion { 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. - #define _DIVCEIL(A,B) (((A) + (B) - 1) / (B)) - static constexpr uint32_t _ftm_ratio = TERN(FTM_UNIFIED_BWS, 2, _DIVCEIL(FTM_WINDOW_SIZE, FTM_BATCH_SIZE)), - shaper_intervals = (FTM_BATCH_SIZE) * _DIVCEIL(FTM_ZMAX, FTM_BATCH_SIZE), - min_max_intervals = (FTM_BATCH_SIZE) * _ftm_ratio; - // Make vector variables. static uint32_t makeVector_idx, - makeVector_idx_z1, makeVector_batchIdx; // Interpolation variables. - static uint32_t interpIdx, - interpIdx_z1; + static uint32_t interpIdx; static xyze_long_t steps; @@ -184,21 +160,20 @@ class FTMotion { float d_zi[FTM_ZMAX] = { 0.0f }; // Data point delay vector. float Ai[5]; // Shaping gain vector. uint32_t Ni[5]; // Shaping time index vector. + uint32_t max_i; // Vector length for the selected shaper. - void updateShapingN(const_float_t f, const_float_t df); + void set_axis_shaping_N(const ftMotionShaper_t shaper, const_float_t f, const_float_t zeta); // Sets the gains used by shaping functions. + void set_axis_shaping_A(const ftMotionShaper_t shaper, const_float_t zeta, const_float_t vtol); // Sets the indices used by shaping functions. } axis_shaping_t; typedef struct Shaping { - uint32_t zi_idx, // Index of storage in the data point delay vectors. - max_i; // Vector length for the selected shaper. + uint32_t zi_idx; // Index of storage in the data point delay vectors. axis_shaping_t x; #if HAS_Y_AXIS axis_shaping_t y; #endif - void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol); - } shaping_t; static shaping_t shaping; // Shaping data @@ -211,6 +186,8 @@ class FTMotion { #endif // Private methods + static void discard_planner_block_protected(); + static void runoutBlock(); static int32_t stepperCmdBuffItems(); static void loadBlockData(block_t *const current_block); static void makeVector(); diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h index e6a25060178..51cd548a03f 100644 --- a/Marlin/src/module/ft_types.h +++ b/Marlin/src/module/ft_types.h @@ -23,18 +23,17 @@ #include "../core/types.h" -typedef enum FXDTICtrlMode : uint8_t { - ftMotionMode_DISABLED = 0, // Standard Motion - ftMotionMode_ENABLED = 1, // Time-Based Motion - ftMotionMode_ZV = 10, // Zero Vibration - ftMotionMode_ZVD = 11, // Zero Vibration and Derivative - ftMotionMode_ZVDD = 12, // Zero Vibration, Derivative, and Double Derivative - ftMotionMode_ZVDDD = 13, // Zero Vibration, Derivative, Double Derivative, and Triple Derivative - ftMotionMode_EI = 14, // Extra-Intensive - ftMotionMode_2HEI = 15, // 2-Hump Extra-Intensive - ftMotionMode_3HEI = 16, // 3-Hump Extra-Intensive - ftMotionMode_MZV = 17 // Mass-based Zero Vibration -} ftMotionMode_t; +typedef enum FXDTICtrlShaper : uint8_t { + ftMotionShaper_NONE = 0, // No compensator + ftMotionShaper_ZV = 1, // Zero Vibration + ftMotionShaper_ZVD = 2, // Zero Vibration and Derivative + ftMotionShaper_ZVDD = 3, // Zero Vibration, Derivative, and Double Derivative + ftMotionShaper_ZVDDD = 4, // Zero Vibration, Derivative, Double Derivative, and Triple Derivative + ftMotionShaper_EI = 5, // Extra-Intensive + 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, @@ -42,7 +41,8 @@ enum dynFreqMode_t : uint8_t { dynFreqMode_MASS_BASED = 2 }; -#define IS_EI_MODE(N) WITHIN(N, ftMotionMode_EI, ftMotionMode_3HEI) +#define CMPNSTR_HAS_SHAPER(A) (ftMotion.cfg.shaper[A] != ftMotionShaper_NONE) +#define CMPNSTR_IS_EISHAPER(A) WITHIN(ftMotion.cfg.shaper[A], ftMotionShaper_EI, ftMotionShaper_3HEI) typedef struct XYZEarray xyze_trajectory_t; typedef struct XYZEarray xyze_trajectoryMod_t; diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index b3dc856a0a5..0773fa40216 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -1584,7 +1584,7 @@ void Planner::quick_stop() { // Restart the block delay for the first movement - As the queue was // forced to empty, there's no risk the ISR will touch this. - delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; TERN_(HAS_WIRED_LCD, clear_block_buffer_runtime()); // Clear the accumulated runtime @@ -1745,7 +1745,7 @@ bool Planner::_buffer_steps(const xyze_long_t &target // As there are no queued movements, the Stepper ISR will not touch this // variable, so there is no risk setting this here (but it MUST be done // before the following line!!) - delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } // Move buffer head @@ -2804,7 +2804,7 @@ void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_PO // As there are no queued movements, the Stepper ISR will not touch this // variable, so there is no risk setting this here (but it MUST be done // before the following line!!) - delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } block_buffer_head = next_buffer_head; @@ -3097,7 +3097,7 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s // As there are no queued movements, the Stepper ISR will not touch this // variable, so there is no risk setting this here (but it MUST be done // before the following line!!) - delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } // Move buffer head @@ -3118,7 +3118,7 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s void Planner::set_machine_position_mm(const abce_pos_t &abce) { // When FT Motion is enabled, call synchronize() here instead of generating a sync block - if (TERN0(FT_MOTION, ftMotion.cfg.mode)) synchronize(); + if (TERN0(FT_MOTION, ftMotion.cfg.active)) synchronize(); TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder); TERN_(HAS_POSITION_FLOAT, position_float = abce); diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 9da12dee118..22dc136839d 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -1531,7 +1531,7 @@ void Stepper::isr() { uint8_t max_loops = 10; #if ENABLED(FT_MOTION) - const bool using_ftMotion = ftMotion.cfg.mode; + const bool using_ftMotion = ftMotion.cfg.active; #else constexpr bool using_ftMotion = false; #endif @@ -2610,7 +2610,7 @@ hal_timer_t Stepper::block_phase_isr() { if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC && planner.laser_inline.status.isPowered // isPowered flag set on any parsed G1, G2, G3, or G5 move; cleared on any others. && current_block // Block may not be available if steps completed (see discard_current_block() above) - && cutter.last_block_power != current_block->laser.power // Only update if the power changed + && cutter.last_block_power != current_block->laser.power // Prevent constant update without change ) { cutter.apply_power(current_block->laser.power); cutter.last_block_power = current_block->laser.power; @@ -3534,8 +3534,6 @@ void Stepper::report_positions() { */ void Stepper::ftMotion_stepper() { - static AxisBits direction_bits{0}; - // Check if the buffer is empty. ftMotion.sts_stepperBusy = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx); if (!ftMotion.sts_stepperBusy) return; @@ -3553,34 +3551,14 @@ void Stepper::report_positions() { #define _FTM_STEP(AXIS) TEST(command, FT_BIT_STEP_##AXIS) #define _FTM_DIR(AXIS) TEST(command, FT_BIT_DIR_##AXIS) - AxisBits axis_step; - axis_step = LOGICAL_AXIS_ARRAY( - TEST(command, FT_BIT_STEP_E), - TEST(command, FT_BIT_STEP_X), TEST(command, FT_BIT_STEP_Y), TEST(command, FT_BIT_STEP_Z), - TEST(command, FT_BIT_STEP_I), TEST(command, FT_BIT_STEP_J), TEST(command, FT_BIT_STEP_K), - TEST(command, FT_BIT_STEP_U), TEST(command, FT_BIT_STEP_V), TEST(command, FT_BIT_STEP_W) - ); + /** + * Set bits in axis_did_move for any axes moving in this block, + * clearing the bits at the start of each new segment. + */ + if (TEST(command, FT_BIT_START)) axis_did_move.reset(); - direction_bits = LOGICAL_AXIS_ARRAY( - axis_step.e ? TEST(command, FT_BIT_DIR_E) : direction_bits.e, - axis_step.x ? TEST(command, FT_BIT_DIR_X) : direction_bits.x, - axis_step.y ? TEST(command, FT_BIT_DIR_Y) : direction_bits.y, - axis_step.z ? TEST(command, FT_BIT_DIR_Z) : direction_bits.z, - axis_step.i ? TEST(command, FT_BIT_DIR_I) : direction_bits.i, - axis_step.j ? TEST(command, FT_BIT_DIR_J) : direction_bits.j, - axis_step.k ? TEST(command, FT_BIT_DIR_K) : direction_bits.k, - axis_step.u ? TEST(command, FT_BIT_DIR_U) : direction_bits.u, - axis_step.v ? TEST(command, FT_BIT_DIR_V) : direction_bits.v, - axis_step.w ? TEST(command, FT_BIT_DIR_W) : direction_bits.w - ); - - // Apply directions (which will apply to the entire linear move) - LOGICAL_AXIS_CODE( - E_APPLY_DIR(direction_bits.e, false), - X_APPLY_DIR(direction_bits.x, false), Y_APPLY_DIR(direction_bits.y, false), Z_APPLY_DIR(direction_bits.z, false), - I_APPLY_DIR(direction_bits.i, false), J_APPLY_DIR(direction_bits.j, false), K_APPLY_DIR(direction_bits.k, false), - U_APPLY_DIR(direction_bits.u, false), V_APPLY_DIR(direction_bits.v, false), W_APPLY_DIR(direction_bits.w, false) - ); + #define _FTM_AXIS_DID_MOVE(AXIS) axis_did_move.bset(_AXIS(AXIS), _FTM_STEP(AXIS)); + LOGICAL_AXIS_MAP(_FTM_AXIS_DID_MOVE); /** * Update direction bits for steppers that were stepped by this command. @@ -3588,13 +3566,8 @@ void Stepper::report_positions() { * when the block was fetched and are not overwritten here. */ - // Start a step pulse - LOGICAL_AXIS_CODE( - E_APPLY_STEP(axis_step.e, false), - X_APPLY_STEP(axis_step.x, false), Y_APPLY_STEP(axis_step.y, false), Z_APPLY_STEP(axis_step.z, false), - I_APPLY_STEP(axis_step.i, false), J_APPLY_STEP(axis_step.j, false), K_APPLY_STEP(axis_step.k, false), - U_APPLY_STEP(axis_step.u, false), V_APPLY_STEP(axis_step.v, false), W_APPLY_STEP(axis_step.w, false) - ); + #define _FTM_SET_DIR(AXIS) if (_FTM_STEP(AXIS)) last_direction_bits.bset(_AXIS(AXIS), _FTM_DIR(AXIS)); + LOGICAL_AXIS_MAP(_FTM_SET_DIR); if (TERN1(FTM_OPTIMIZE_DIR_STATES, last_set_direction != last_direction_bits)) { // Apply directions (generally applying to the entire linear move) @@ -3619,7 +3592,7 @@ void Stepper::report_positions() { START_TIMED_PULSE(); // Update step counts - #define _FTM_STEP_COUNT(AXIS) if (axis_step[_AXIS(AXIS)]) count_position[_AXIS(AXIS)] += direction_bits[_AXIS(AXIS)] ? 1 : -1; + #define _FTM_STEP_COUNT(AXIS) if (_FTM_STEP(AXIS)) count_position[_AXIS(AXIS)] += last_direction_bits[_AXIS(AXIS)] ? 1 : -1; LOGICAL_AXIS_MAP(_FTM_STEP_COUNT); // Provide EDGE flags for E stepper(s) @@ -3635,10 +3608,10 @@ void Stepper::report_positions() { // Only wait for axes without edge stepping const bool any_wait = false LOGICAL_AXIS_GANG( - || (!e_axis_has_dedge && axis_step.e), - || (!AXIS_HAS_DEDGE(X) && axis_step.x), || (!AXIS_HAS_DEDGE(Y) && axis_step.y), || (!AXIS_HAS_DEDGE(Z) && axis_step.z), - || (!AXIS_HAS_DEDGE(I) && axis_step.i), || (!AXIS_HAS_DEDGE(J) && axis_step.j), || (!AXIS_HAS_DEDGE(K) && axis_step.k), - || (!AXIS_HAS_DEDGE(U) && axis_step.u), || (!AXIS_HAS_DEDGE(V) && axis_step.v), || (!AXIS_HAS_DEDGE(W) && axis_step.w) + || (!e_axis_has_dedge && _FTM_STEP(E)), + || (!AXIS_HAS_DEDGE(X) && _FTM_STEP(X)), || (!AXIS_HAS_DEDGE(Y) && _FTM_STEP(Y)), || (!AXIS_HAS_DEDGE(Z) && _FTM_STEP(Z)), + || (!AXIS_HAS_DEDGE(I) && _FTM_STEP(I)), || (!AXIS_HAS_DEDGE(J) && _FTM_STEP(J)), || (!AXIS_HAS_DEDGE(K) && _FTM_STEP(K)), + || (!AXIS_HAS_DEDGE(U) && _FTM_STEP(U)), || (!AXIS_HAS_DEDGE(V) && _FTM_STEP(V)), || (!AXIS_HAS_DEDGE(W) && _FTM_STEP(W)) ); // Allow pulses to be registered by stepper drivers @@ -3648,83 +3621,13 @@ void Stepper::report_positions() { #define _FTM_STEP_STOP(AXIS) AXIS##_APPLY_STEP(!STEP_STATE_##AXIS, false); LOGICAL_AXIS_MAP(_FTM_STEP_STOP); - // Also handle babystepping here - TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr()); + // Check endstops on every step using axis_did_move as set by every step + // TODO: Update endstop states less frequently to save processing. + // NOTE: endstops.poll is still called at 1KHz by Temperature ISR. + IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, if ((bool)axis_did_move) endstops.update()); } // Stepper::ftMotion_stepper - // Called from FTMotion::loop (when !blockProcRdy) which is called from Marlin idle() - void Stepper::ftMotion_blockQueueUpdate() { - - if (current_block) { - // If the current block is not done processing, return right away. - // A block is done processing when the command buffer has been - // filled, not necessarily when it's done running. - if (!ftMotion.getBlockProcDn()) return; - planner.release_current_block(); - } - - // Check the buffer for a new block - current_block = planner.get_current_block(); - - if (current_block) { - - // Sync position, fan power, laser power? - while (current_block->is_sync()) { - - #if 0 - - // TODO: Implement compatible sync blocks with FT Motion commands, - // perhaps by setting a FT_BIT_SYNC flag that holds the current block - // until it is processed by ftMotion_stepper - - // Set laser power - #if ENABLED(LASER_POWER_SYNC) - if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) { - if (current_block->is_sync_pwr()) { - planner.laser_inline.status.isSyncPower = true; - cutter.apply_power(current_block->laser.power); - } - } - #endif - - // Set "fan speeds" for a laser module - #if ENABLED(LASER_SYNCHRONOUS_M106_M107) - if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed); - #endif - - // Set position - if (current_block->is_sync_pos()) _set_position(current_block->position); - - #endif - - // Done with this block - planner.release_current_block(); - - // Try to get a new block - if (!(current_block = planner.get_current_block())) - return; // No queued blocks. - } - - // Some kinematics track axis motion in HX, HY, HZ - #if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX) - last_direction_bits.hx = current_block->direction_bits.hx; - #endif - #if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX) - last_direction_bits.hy = current_block->direction_bits.hy; - #endif - #if ANY(CORE_IS_XZ, CORE_IS_YZ) - last_direction_bits.hz = current_block->direction_bits.hz; - #endif - - ftMotion.startBlockProc(); - return; - } - - ftMotion.runoutBlock(); - - } // Stepper::ftMotion_blockQueueUpdate() - #endif // FT_MOTION #if ENABLED(BABYSTEPPING) diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 2a171bebd0b..46b3493bb6e 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -668,8 +668,6 @@ class Stepper { } #if ENABLED(FT_MOTION) - // Manage the planner - static void ftMotion_blockQueueUpdate(); // Set current position in steps when reset flag is set in M493 and planner already synchronized static void ftMotion_syncPosition(); #endif