From c37fa3cc9097a9aa7fad5f168e335caabf23278e Mon Sep 17 00:00:00 2001 From: Scott Lahteine Date: Fri, 31 Mar 2023 21:18:37 -0500 Subject: [PATCH] =?UTF-8?q?=E2=9C=A8=20Fixed-Time=20Motion=20with=20Input?= =?UTF-8?q?=20Shaping=20by=20Ulendo=20(#25394)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Ulendo Alex --- Marlin/Configuration_adv.h | 48 +- Marlin/src/MarlinCore.cpp | 7 + Marlin/src/gcode/feature/ft_motion/M493.cpp | 282 ++++++ Marlin/src/gcode/gcode.cpp | 6 +- Marlin/src/gcode/gcode.h | 4 + Marlin/src/module/ft_motion.cpp | 924 ++++++++++++++++++++ Marlin/src/module/ft_motion.h | 170 ++++ Marlin/src/module/ft_types.h | 59 ++ Marlin/src/module/planner.cpp | 22 +- Marlin/src/module/planner.h | 4 + Marlin/src/module/stepper.cpp | 275 +++++- Marlin/src/module/stepper.h | 15 + buildroot/tests/STM32F103RC_btt | 4 +- ini/features.ini | 1 + platformio.ini | 1 + 15 files changed, 1772 insertions(+), 50 deletions(-) create mode 100644 Marlin/src/gcode/feature/ft_motion/M493.cpp create mode 100644 Marlin/src/module/ft_motion.cpp create mode 100644 Marlin/src/module/ft_motion.h create mode 100644 Marlin/src/module/ft_types.h diff --git a/Marlin/Configuration_adv.h b/Marlin/Configuration_adv.h index 070309d5aa..18e5d9e647 100644 --- a/Marlin/Configuration_adv.h +++ b/Marlin/Configuration_adv.h @@ -1086,7 +1086,51 @@ #endif -// @section motion +// @section motion control + +/** + * Fixed-time-based Motion Control -- EXPERIMENTAL + * Enable/disable and set parameters with G-code M493. + */ +//#define FT_MOTION +#if ENABLED(FT_MOTION) + #define FTM_DEFAULT_MODE ftMotionMode_ENABLED // 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_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). + #define FTM_LINEAR_ADV_DEFAULT_K 0.0f // Default linear advance gain. + #define FTM_SHAPING_ZETA 0.1f // Zeta used by input shapers. + #define FTM_SHAPING_V_TOL 0.05f // Vibration tolerance used by EI input shapers. + + /** + * Advanced configuration + */ + #define FTM_BATCH_SIZE 100 // Batch size for trajectory generation; + // half the window size for Ulendo FBS. + #define FTM_FS 1000 // (Hz) Frequency for trajectory generation. (1 / FTM_TS) + #define FTM_TS 0.001f // (s) Time step for trajectory generation. (1 / FTM_FS) + #define FTM_STEPPER_FS 20000 // (Hz) Frequency for stepper I/O update. + #define FTM_MIN_TICKS ((STEPPER_TIMER_RATE) / (FTM_STEPPER_FS)) // Minimum stepper ticks between steps. + #define FTM_MIN_SHAPE_FREQ 10 // Minimum shaping frequency. + #define FTM_ZMAX 100 // Maximum delays for shaping functions (even numbers only!). + // Calculate as: + // 1/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZV. + // (FTM_FS / FTM_MIN_SHAPE_FREQ) for ZVD, MZV. + // 3/2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 2HEI. + // 2 * (FTM_FS / FTM_MIN_SHAPE_FREQ) for 3HEI. + #define FTM_STEPS_PER_UNIT_TIME 20 // Interpolated stepper commands per unit time. + // Calculate as (FTM_STEPPER_FS / FTM_FS). + #define FTM_CTS_COMPARE_VAL 10 // Comparison value used in interpolation algorithm. + // Calculate as (FTM_STEPS_PER_UNIT_TIME / 2). + // These values may be configured to adjust 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(). + + // This value may be configured to adjust duration to consume the command buffer. + // Try increasing this value if stepper motion is not smooth. + #define FTM_STEPPERCMD_BUFF_SIZE 1000 // Size of the stepper command buffers. +#endif /** * Input Shaping -- EXPERIMENTAL @@ -1125,6 +1169,8 @@ //#define SHAPING_MENU // Add a menu to the LCD to set shaping parameters. #endif +// @section motion + #define AXIS_RELATIVE_MODES { false, false, false, false } // Add a Duplicate option for well-separated conjoined nozzles diff --git a/Marlin/src/MarlinCore.cpp b/Marlin/src/MarlinCore.cpp index 213cbebc26..19aaedf7c6 100644 --- a/Marlin/src/MarlinCore.cpp +++ b/Marlin/src/MarlinCore.cpp @@ -50,6 +50,9 @@ #include "module/settings.h" #include "module/stepper.h" #include "module/temperature.h" +#if ENABLED(FT_MOTION) + #include "module/ft_motion.h" +#endif #include "gcode/gcode.h" #include "gcode/parser.h" @@ -885,8 +888,12 @@ void idle(bool no_stepper_sleep/*=false*/) { // Update the LVGL interface TERN_(HAS_TFT_LVGL_UI, LV_TASK_HANDLER()); + // Manage Fixed-time Motion Control + TERN_(FT_MOTION, fxdTiCtrl.loop()); + IDLE_DONE: TERN_(MARLIN_DEV_MODE, idle_depth--); + return; } diff --git a/Marlin/src/gcode/feature/ft_motion/M493.cpp b/Marlin/src/gcode/feature/ft_motion/M493.cpp new file mode 100644 index 0000000000..31e16a194d --- /dev/null +++ b/Marlin/src/gcode/feature/ft_motion/M493.cpp @@ -0,0 +1,282 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../../../inc/MarlinConfig.h" + +#if ENABLED(FT_MOTION) + +#include "../../gcode.h" +#include "../../../module/ft_motion.h" + +void say_shaping() { + SERIAL_ECHO_TERNARY(fxdTiCtrl.cfg_mode, "Fixed time controller ", "en", "dis", "abled"); + if (fxdTiCtrl.cfg_mode == ftMotionMode_DISABLED || fxdTiCtrl.cfg_mode == ftMotionMode_ENABLED) { + SERIAL_ECHOLNPGM("."); + return; + } + #if HAS_X_AXIS + SERIAL_ECHOPGM(" with "); + switch (fxdTiCtrl.cfg_mode) { + default: break; + //case ftMotionMode_ULENDO_FBS: SERIAL_ECHOLNPGM("Ulendo FBS."); return; + case ftMotionMode_ZV: SERIAL_ECHOLNPGM("ZV"); break; + case ftMotionMode_ZVD: SERIAL_ECHOLNPGM("ZVD"); break; + case ftMotionMode_EI: SERIAL_ECHOLNPGM("EI"); break; + case ftMotionMode_2HEI: SERIAL_ECHOLNPGM("2 Hump EI"); break; + case ftMotionMode_3HEI: SERIAL_ECHOLNPGM("3 Hump EI"); break; + case ftMotionMode_MZV: SERIAL_ECHOLNPGM("MZV"); break; + //case ftMotionMode_DISCTF: SERIAL_ECHOLNPGM("discrete transfer functions"); break; + } + SERIAL_ECHOLNPGM(" shaping."); + #endif +} + +/** + * M493: Set Fixed-time Motion Control parameters + * + * S Set the motion / shaping mode. Shaping requires an X axis, at the minimum. + * 0: NORMAL + * 1: FIXED-TIME + * 10: ZV + * 11: ZVD + * 12: EI + * 13: 2HEI + * 14: 3HEI + * 15: MZV + * + * P Enable (1) or Disable (0) Linear Advance pressure control + * + * K Set Linear Advance gain + * + * D Set Dynamic Frequency mode + * 0: DISABLED + * 1: Z-based (Requires a Z axis) + * 2: Mass-based (Requires X and E axes) + * + * A Set static/base frequency for the X axis + * F Set frequency scaling for the X axis + * + * B Set static/base frequency for the Y axis + * H Set frequency scaling for the Y axis + */ +void GcodeSuite::M493() { + // Parse 'S' mode parameter. + if (parser.seenval('S')) { + const ftMotionMode_t val = (ftMotionMode_t)parser.value_byte(); + switch (val) { + case ftMotionMode_DISABLED: + case ftMotionMode_ENABLED: + #if HAS_X_AXIS + case ftMotionMode_ZVD: + case ftMotionMode_2HEI: + case ftMotionMode_3HEI: + case ftMotionMode_MZV: + //case ftMotionMode_ULENDO_FBS: + //case ftMotionMode_DISCTF: + fxdTiCtrl.cfg_mode = val; + say_shaping(); + break; + #endif + default: + SERIAL_ECHOLNPGM("?Invalid control mode [M] value."); + return; + } + + switch (val) { + case ftMotionMode_ENABLED: fxdTiCtrl.reset(); break; + #if HAS_X_AXIS + case ftMotionMode_ZV: + case ftMotionMode_ZVD: + case ftMotionMode_EI: + case ftMotionMode_2HEI: + case ftMotionMode_3HEI: + case ftMotionMode_MZV: + fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1])); + fxdTiCtrl.updateShapingA(); + fxdTiCtrl.reset(); + break; + //case ftMotionMode_ULENDO_FBS: + //case ftMotionMode_DISCTF: + #endif + default: break; + } + } + + #if HAS_EXTRUDERS + + // Pressure control (linear advance) parameter. + if (parser.seen('P')) { + const bool val = parser.value_bool(); + fxdTiCtrl.cfg_linearAdvEna = val; + SERIAL_ECHO_TERNARY(val, "Pressure control: Linear Advance ", "en", "dis", "abled.\n"); + } + + // Pressure control (linear advance) gain parameter. + if (parser.seenval('K')) { + const float val = parser.value_float(); + if (val >= 0.0f) { + fxdTiCtrl.cfg_linearAdvK = val; + SERIAL_ECHOPGM("Pressure control: Linear Advance gain set to: "); + SERIAL_ECHO_F(val, 5); + SERIAL_ECHOLNPGM("."); + } + else { // Value out of range. + SERIAL_ECHOLNPGM("Pressure control: Linear Advance gain out of range."); + } + } + + #endif // HAS_EXTRUDERS + + #if HAS_Z_AXIS || HAS_EXTRUDERS + + // Dynamic frequency mode parameter. + if (parser.seenval('D')) { + if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) { + const dynFreqMode_t val = dynFreqMode_t(parser.value_byte()); + switch (val) { + case dynFreqMode_DISABLED: + fxdTiCtrl.cfg_dynFreqMode = val; + SERIAL_ECHOLNPGM("Dynamic frequency mode disabled."); + break; + #if HAS_Z_AXIS + case dynFreqMode_Z_BASED: + fxdTiCtrl.cfg_dynFreqMode = val; + SERIAL_ECHOLNPGM("Z-based Dynamic Frequency Mode."); + break; + #endif + #if HAS_EXTRUDERS + case dynFreqMode_MASS_BASED: + fxdTiCtrl.cfg_dynFreqMode = val; + SERIAL_ECHOLNPGM("Mass-based Dynamic Frequency Mode."); + break; + #endif + default: + SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value."); + break; + } + } + else { + SERIAL_ECHOLNPGM("Incompatible shaper for [D] Dynamic Frequency mode."); + } + } + + #endif // HAS_Z_AXIS || HAS_EXTRUDERS + + #if HAS_X_AXIS + + // Parse frequency parameter (X axis). + if (parser.seenval('A')) { + if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) { + const float val = parser.value_float(); + const bool frequencyInRange = WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2); + // TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct. + if (frequencyInRange) { + fxdTiCtrl.cfg_baseFreq[0] = val; + fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1])); + fxdTiCtrl.reset(); + if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (X/A axis) set to:"); } + else { SERIAL_ECHOPGM("Compensator static frequency (X/A axis) set to: "); } + SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[0], 2 ); + SERIAL_ECHOLNPGM("."); + } + else { // Frequency out of range. + SERIAL_ECHOLNPGM("Invalid [A] frequency value."); + } + } + else { // Mode doesn't use frequency. + SERIAL_ECHOLNPGM("Incompatible mode for [A] frequency."); + } + } + + #if HAS_Z_AXIS || HAS_EXTRUDERS + // Parse frequency scaling parameter (X axis). + if (parser.seenval('F')) { + const bool modeUsesDynFreq = ( + TERN0(HAS_Z_AXIS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_Z_BASED) + || TERN0(HAS_EXTRUDERS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_MASS_BASED) + ); + + if (modeUsesDynFreq) { + const float val = parser.value_float(); + fxdTiCtrl.cfg_dynFreqK[0] = val; + SERIAL_ECHOPGM("Frequency scaling (X/A axis) set to: "); + SERIAL_ECHO_F(fxdTiCtrl.cfg_dynFreqK[0], 8); + SERIAL_ECHOLNPGM("."); + } + else { + SERIAL_ECHOLNPGM("Incompatible mode for [F] frequency scaling."); + } + } + #endif // HAS_Z_AXIS || HAS_EXTRUDERS + + #endif // HAS_X_AXIS + + #if HAS_Y_AXIS + + // Parse frequency parameter (Y axis). + if (parser.seenval('B')) { + if (WITHIN(fxdTiCtrl.cfg_mode, 10U, 19U)) { + const float val = parser.value_float(); + const bool frequencyInRange = WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2); + if (frequencyInRange) { + fxdTiCtrl.cfg_baseFreq[1] = val; + fxdTiCtrl.updateShapingN(fxdTiCtrl.cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, fxdTiCtrl.cfg_baseFreq[1])); + fxdTiCtrl.reset(); + if (fxdTiCtrl.cfg_dynFreqMode) { SERIAL_ECHOPGM("Compensator base dynamic frequency (Y/B axis) set to:"); } + else { SERIAL_ECHOPGM("Compensator static frequency (Y/B axis) set to: "); } + SERIAL_ECHO_F( fxdTiCtrl.cfg_baseFreq[1], 2 ); + SERIAL_ECHOLNPGM("."); + } + else { // Frequency out of range. + SERIAL_ECHOLNPGM("Invalid frequency [B] value."); + } + } + else { // Mode doesn't use frequency. + SERIAL_ECHOLNPGM("Incompatible mode for [B] frequency."); + } + } + + #if HAS_Z_AXIS || HAS_EXTRUDERS + // Parse frequency scaling parameter (Y axis). + if (parser.seenval('H')) { + const bool modeUsesDynFreq = ( + TERN0(HAS_Z_AXIS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_Z_BASED) + || TERN0(HAS_EXTRUDERS, fxdTiCtrl.cfg_dynFreqMode == dynFreqMode_MASS_BASED) + ); + + if (modeUsesDynFreq) { + const float val = parser.value_float(); + fxdTiCtrl.cfg_dynFreqK[1] = val; + SERIAL_ECHOPGM("Frequency scaling (Y/B axis) set to: "); + SERIAL_ECHO_F(val, 8); + SERIAL_ECHOLNPGM("."); + } + else { + SERIAL_ECHOLNPGM("Incompatible mode for [H] frequency scaling."); + } + } + #endif // HAS_Z_AXIS || HAS_EXTRUDERS + + #endif // HAS_Y_AXIS +} + +#endif // FT_MOTION diff --git a/Marlin/src/gcode/gcode.cpp b/Marlin/src/gcode/gcode.cpp index 1752ae8a18..0711d39204 100644 --- a/Marlin/src/gcode/gcode.cpp +++ b/Marlin/src/gcode/gcode.cpp @@ -895,6 +895,10 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) { case 486: M486(); break; // M486: Identify and cancel objects #endif + #if ENABLED(FT_MOTION) + case 493: M493(); break; // M493: Fixed-Time Motion control + #endif + case 500: M500(); break; // M500: Store settings in EEPROM case 501: M501(); break; // M501: Read settings from EEPROM case 502: M502(); break; // M502: Revert to default settings @@ -934,7 +938,7 @@ void GcodeSuite::process_parsed_command(const bool no_ok/*=false*/) { #endif #if HAS_ZV_SHAPING - case 593: M593(); break; // M593: Set Input Shaping parameters + case 593: M593(); break; // M593: Input Shaping control #endif #if ENABLED(ADVANCED_PAUSE_FEATURE) diff --git a/Marlin/src/gcode/gcode.h b/Marlin/src/gcode/gcode.h index cc3f221a82..8493d7f291 100644 --- a/Marlin/src/gcode/gcode.h +++ b/Marlin/src/gcode/gcode.h @@ -1038,6 +1038,10 @@ private: static void M486(); #endif + #if ENABLED(FT_MOTION) + static void M493(); + #endif + static void M500(); static void M501(); static void M502(); diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp new file mode 100644 index 0000000000..dfef961c79 --- /dev/null +++ b/Marlin/src/module/ft_motion.cpp @@ -0,0 +1,924 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ + +#include "../inc/MarlinConfig.h" + +#if ENABLED(FT_MOTION) + +#include "ft_motion.h" +#include "stepper.h" // Access stepper block queue function and abort status. + +FxdTiCtrl fxdTiCtrl; + +//-----------------------------------------------------------------// +// Variables. +//-----------------------------------------------------------------// + +// Public variables. +ftMotionMode_t FxdTiCtrl::cfg_mode = FTM_DEFAULT_MODE; // Mode / active compensation mode configuration. + +#if HAS_EXTRUDERS + bool FxdTiCtrl::cfg_linearAdvEna = FTM_LINEAR_ADV_DEFAULT_ENA; // Linear advance enable configuration. + float FxdTiCtrl::cfg_linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; // Linear advance gain. +#endif + +dynFreqMode_t FxdTiCtrl::cfg_dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration. +#if !HAS_Z_AXIS + static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis."); +#endif +#if !(HAS_X_AXIS && HAS_EXTRUDERS) + static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_MASS_BASED, "dynFreqMode_MASS_BASED requires an X axis and an extruder."); +#endif + +#if HAS_X_AXIS + float FxdTiCtrl::cfg_baseFreq[] = { FTM_SHAPING_DEFAULT_X_FREQ // Base frequency. [Hz] + OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) }; + float FxdTiCtrl::cfg_dynFreqK[] = { 0.0f OPTARG(HAS_Y_AXIS, 0.0f) }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g] +#endif + +ft_command_t FxdTiCtrl::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of stepper commands. +hal_timer_t FxdTiCtrl::stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of the stepper command timing. +uint8_t FxdTiCtrl::stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE] = {0U}; // Buffer of whether DIR needs to be updated. +uint32_t FxdTiCtrl::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer. + FxdTiCtrl::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer. + +bool FxdTiCtrl::sts_stepperBusy = false; // The stepper buffer has items and is in use. + +// Private variables. +// NOTE: These are sized for Ulendo FBS use. +#if HAS_X_AXIS + float FxdTiCtrl::xd[2 * (FTM_BATCH_SIZE)], // = {0.0f} Storage for fixed-time-based trajectory. + FxdTiCtrl::xm[FTM_BATCH_SIZE]; // = {0.0f} Storage for modified fixed-time-based trajectory. +#endif +#if HAS_Y_AXIS + float FxdTiCtrl::yd[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::ym[FTM_BATCH_SIZE]; +#endif +#if HAS_Z_AXIS + float FxdTiCtrl::zd[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::zm[FTM_BATCH_SIZE]; +#endif +#if HAS_EXTRUDERS + float FxdTiCtrl::ed[2 * (FTM_BATCH_SIZE)], FxdTiCtrl::em[FTM_BATCH_SIZE]; +#endif + +block_t* FxdTiCtrl::current_block_cpy = nullptr; // Pointer to current block being processed. +bool FxdTiCtrl::blockProcRdy = false, // Indicates a block is ready to be processed. + FxdTiCtrl::blockProcRdy_z1 = false, // Storage for the previous indicator. + FxdTiCtrl::blockProcDn = false; // Indicates current block is done being processed. +bool FxdTiCtrl::batchRdy = false; // Indicates a batch of the fixed time trajectory + // has been generated, is now available in the upper - + // half of xd, yd, zd, ed vectors, and is ready to be + // post processed, if applicable, then interpolated. +bool FxdTiCtrl::batchRdyForInterp = false; // Indicates the batch is done being post processed, + // if applicable, and is ready to be converted to step commands. +bool FxdTiCtrl::runoutEna = false; // True if runout of the block hasn't been done and is allowed. + +// Trapezoid data variables. +#if HAS_X_AXIS + float FxdTiCtrl::x_startPosn, // (mm) Start position of block + FxdTiCtrl::x_endPosn_prevBlock = 0.0f, // (mm) Start position of block + FxdTiCtrl::x_Ratio; // (ratio) Axis move ratio of block +#endif +#if HAS_Y_AXIS + float FxdTiCtrl::y_startPosn, + FxdTiCtrl::y_endPosn_prevBlock = 0.0f, + FxdTiCtrl::y_Ratio; +#endif +#if HAS_Z_AXIS + float FxdTiCtrl::z_startPosn, + FxdTiCtrl::z_endPosn_prevBlock = 0.0f, + FxdTiCtrl::z_Ratio; +#endif +#if HAS_EXTRUDERS + float FxdTiCtrl::e_startPosn, + FxdTiCtrl::e_endPosn_prevBlock = 0.0f, + FxdTiCtrl::e_Ratio; +#endif +float FxdTiCtrl::accel_P, // Acceleration prime of block. [mm/sec/sec] + FxdTiCtrl::decel_P, // Deceleration prime of block. [mm/sec/sec] + FxdTiCtrl::F_P, // Feedrate prime of block. [mm/sec] + FxdTiCtrl::f_s, // Starting feedrate of block. [mm/sec] + FxdTiCtrl::s_1e, // Position after acceleration phase of block. + FxdTiCtrl::s_2e; // Position after acceleration and coasting phase of block. + +uint32_t FxdTiCtrl::N1, // Number of data points in the acceleration phase. + FxdTiCtrl::N2, // Number of data points in the coasting phase. + FxdTiCtrl::N3; // Number of data points in the deceleration phase. + +uint32_t FxdTiCtrl::max_intervals; // Total number of data points that will be generated from block. + +// Make vector variables. +uint32_t FxdTiCtrl::makeVector_idx = 0, // Index of fixed time trajectory generation of the overall block. + FxdTiCtrl::makeVector_idx_z1 = 0, // Storage for the previously calculated index above. + FxdTiCtrl::makeVector_batchIdx = FTM_BATCH_SIZE; // Index of fixed time trajectory generation within the batch. + +// Interpolation variables. +#if HAS_X_AXIS + int32_t FxdTiCtrl::x_steps = 0; // Step count accumulator. + stepDirState_t FxdTiCtrl::x_dirState = stepDirState_NOT_SET; // Memory of the currently set step direction of the axis. +#endif +#if HAS_Y_AXIS + int32_t FxdTiCtrl::y_steps = 0; + stepDirState_t FxdTiCtrl::y_dirState = stepDirState_NOT_SET; +#endif +#if HAS_Z_AXIS + int32_t FxdTiCtrl::z_steps = 0; + stepDirState_t FxdTiCtrl::z_dirState = stepDirState_NOT_SET; +#endif +#if HAS_EXTRUDERS + int32_t FxdTiCtrl::e_steps = 0; + stepDirState_t FxdTiCtrl::e_dirState = stepDirState_NOT_SET; +#endif + +uint32_t FxdTiCtrl::interpIdx = 0, // Index of current data point being interpolated. + FxdTiCtrl::interpIdx_z1 = 0; // Storage for the previously calculated index above. +hal_timer_t FxdTiCtrl::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the next step time (in ticks). + +// Shaping variables. +#if HAS_X_AXIS + uint32_t FxdTiCtrl::xy_zi_idx = 0, // Index of storage in the data point delay vectors. + FxdTiCtrl::xy_max_i = 0; // Vector length for the selected shaper. + float FxdTiCtrl::xd_zi[FTM_ZMAX] = { 0.0f }; // Data point delay vector. + float FxdTiCtrl::x_Ai[5]; // Shaping gain vector. + uint32_t FxdTiCtrl::x_Ni[5]; // Shaping time index vector. +#endif +#if HAS_Y_AXIS + float FxdTiCtrl::yd_zi[FTM_ZMAX] = { 0.0f }; + float FxdTiCtrl::y_Ai[5]; + uint32_t FxdTiCtrl::y_Ni[5]; +#endif + +#if HAS_EXTRUDERS + // Linear advance variables. + float FxdTiCtrl::e_raw_z1 = 0.0f; // (ms) Unit delay of raw extruder position. + float FxdTiCtrl::e_advanced_z1 = 0.0f; // (ms) Unit delay of advanced extruder position. +#endif + +//-----------------------------------------------------------------// +// Function definitions. +//-----------------------------------------------------------------// + +// Public functions. + +// Sets controller states to begin processing a block. +void FxdTiCtrl::startBlockProc(block_t * const current_block) { + current_block_cpy = current_block; + blockProcRdy = true; + blockProcDn = false; + runoutEna = true; +} + +// Moves any free data points to the stepper buffer even if a full batch isn't ready. +void FxdTiCtrl::runoutBlock() { + + if (runoutEna && !batchRdy) { // If the window is full already (block intervals was a multiple of + // the batch size), or runout is not enabled, no runout is needed. + // Fill out the trajectory window with the last position calculated. + if (makeVector_batchIdx > FTM_BATCH_SIZE) { + for (uint32_t i = makeVector_batchIdx; i < 2 * (FTM_BATCH_SIZE); i++) { + xd[i] = xd[makeVector_batchIdx - 1]; + TERN_(HAS_Y_AXIS, yd[i] = yd[makeVector_batchIdx - 1]); + TERN_(HAS_Y_AXIS, zd[i] = zd[makeVector_batchIdx - 1]); + TERN_(HAS_EXTRUDERS, ed[i] = ed[makeVector_batchIdx - 1]); + } + } + makeVector_batchIdx = FTM_BATCH_SIZE; + batchRdy = true; + } + runoutEna = false; +} + +// Controller main, to be invoked from non-isr task. +void FxdTiCtrl::loop() { + + if (!cfg_mode) return; + + static bool initd = false; + if (!initd) { init(); initd = true; } + + // Handle block abort with the following sequence: + // 1. Zero out commands in stepper ISR. + // 2. Drain the motion buffer, stop processing until they are emptied. + // 3. Reset all the states / memory. + // 4. Signal ready for new block. + if (stepper.abort_current_block) { + if (sts_stepperBusy) return; // Wait until motion buffers are emptied + reset(); + blockProcDn = true; // Set queueing to look for next block. + runoutEna = false; // Disabling running out this block, since we want to halt the motion. + stepper.abort_current_block = false; // Abort finished. + } + + // Planner processing and block conversion. + if (!blockProcRdy) stepper.fxdTiCtrl_BlockQueueUpdate(); + + if (blockProcRdy) { + if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot. + while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP))) + makeVector(); + } + + // FBS / post processing. + if (batchRdy && !batchRdyForInterp) { + + // Call Ulendo FBS here. + + memcpy(xm, &xd[FTM_BATCH_SIZE], sizeof(xm)); + TERN_(HAS_Y_AXIS, memcpy(ym, &yd[FTM_BATCH_SIZE], sizeof(ym))); + + // Done compensating ... + + // Copy the uncompensated vectors. + TERN_(HAS_Z_AXIS, memcpy(zm, &zd[FTM_BATCH_SIZE], sizeof(zm))); + TERN_(HAS_EXTRUDERS, memcpy(em, &ed[FTM_BATCH_SIZE], sizeof(em))); + + // Shift the time series back in the window. + memcpy(xd, &xd[FTM_BATCH_SIZE], sizeof(xd) / 2); + TERN_(HAS_Y_AXIS, memcpy(yd, &yd[FTM_BATCH_SIZE], sizeof(yd) / 2)); + // Disabled by comment as these are uncompensated, the lower half is not used. + //TERN_(HAS_Z_AXIS, memcpy(zd, &zd[FTM_BATCH_SIZE], (sizeof(zd) / 2))); + //TERN_(HAS_EXTRUDERS, memcpy(ed, &ed[FTM_BATCH_SIZE], (sizeof(ed) / 2))); + + // ... data is ready in xm, ym, zm, em. + batchRdyForInterp = true; + + batchRdy = false; // Clear so that makeVector() may resume generating points. + + } // if (batchRdy && !batchRdyForInterp) + + // Interpolation. + while ( batchRdyForInterp + && ( stepperCmdBuffItems() < ((FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME)) ) + && ( (interpIdx - interpIdx_z1) < (FTM_STEPS_PER_LOOP) ) + ) { + convertToSteps(interpIdx); + + if (++interpIdx == FTM_BATCH_SIZE) { + batchRdyForInterp = false; + interpIdx = 0; + } + } + + // Report busy status to planner. + planner.fxdTiCtrl_busy = (sts_stepperBusy || ((!blockProcDn && blockProcRdy) || batchRdy || batchRdyForInterp || runoutEna)); + + 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 FxdTiCtrl::updateShapingA(const_float_t zeta/*=FTM_SHAPING_ZETA*/, const_float_t vtol/*=FTM_SHAPING_V_TOL*/) { + + const float K = exp( -zeta * PI / sqrt(1.0f - sq(zeta)) ), + K2 = sq(K); + + switch (cfg_mode) { + + case ftMotionMode_ZV: + xy_max_i = 1U; + x_Ai[0] = 1.0f / (1.0f + K); + x_Ai[1] = x_Ai[0] * K; + break; + + case ftMotionMode_ZVD: + xy_max_i = 2U; + x_Ai[0] = 1.0f / ( 1.0f + 2.0f * K + K2 ); + x_Ai[1] = x_Ai[0] * 2.0f * K; + x_Ai[2] = x_Ai[0] * K2; + break; + + case ftMotionMode_EI: { + xy_max_i = 2U; + x_Ai[0] = 0.25f * (1.0f + vtol); + x_Ai[1] = 0.50f * (1.0f - vtol) * K; + x_Ai[2] = x_Ai[0] * K2; + const float A_adj = 1.0f / (x_Ai[0] + x_Ai[1] + x_Ai[2]); + for (uint32_t i = 0U; i < 3U; i++) { x_Ai[i] *= A_adj; } + } break; + + case ftMotionMode_2HEI: { + xy_max_i = 3U; + const float vtol2 = sq(vtol); + const float X = pow(vtol2 * (sqrt(1.0f - vtol2) + 1.0f), 1.0f / 3.0f); + x_Ai[0] = ( 3.0f * sq(X) + 2.0f * X + 3.0f * vtol2 ) / (16.0f * X); + x_Ai[1] = ( 0.5f - x_Ai[0] ) * K; + x_Ai[2] = x_Ai[1] * K; + x_Ai[3] = x_Ai[0] * cu(K); + const float A_adj = 1.0f / (x_Ai[0] + x_Ai[1] + x_Ai[2] + x_Ai[3]); + for (uint32_t i = 0U; i < 4U; i++) { x_Ai[i] *= A_adj; } + } break; + + case ftMotionMode_3HEI: { + xy_max_i = 4U; + x_Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) ); + x_Ai[1] = 0.25f * ( 1.0f - vtol ) * K; + x_Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * x_Ai[0] ) * K2; + x_Ai[3] = x_Ai[1] * K2; + x_Ai[4] = x_Ai[0] * sq(K2); + const float A_adj = 1.0f / (x_Ai[0] + x_Ai[1] + x_Ai[2] + x_Ai[3] + x_Ai[4]); + for (uint32_t i = 0U; i < 5U; i++) { x_Ai[i] *= A_adj; } + } break; + + case ftMotionMode_MZV: { + xy_max_i = 2U; + const float B = 1.4142135623730950488016887242097f * K; + x_Ai[0] = 1.0f / (1.0f + B + K2); + x_Ai[1] = x_Ai[0] * B; + x_Ai[2] = x_Ai[0] * K2; + } break; + + default: + for (uint32_t i = 0U; i < 5U; i++) x_Ai[i] = 0.0f; + xy_max_i = 0; + } + #if HAS_Y_AXIS + memcpy(y_Ai, x_Ai, sizeof(x_Ai)); // For now, zeta and vtol are shared across x and y. + #endif + } + + // Refresh the indices used by shaping functions. + // To be called when frequencies change. + void FxdTiCtrl::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), const_float_t zeta/*=FTM_SHAPING_ZETA*/) { + + // Protections omitted for DBZ and for index exceeding array length. + + const float df = sqrt(1.0f - sq(zeta)); + + switch (cfg_mode) { + case ftMotionMode_ZV: + x_Ni[1] = round((0.5f / xf / df) * (FTM_FS)); + #if HAS_Y_AXIS + y_Ni[1] = round((0.5f / yf / df) * (FTM_FS)); + #endif + break; + case ftMotionMode_ZVD: + case ftMotionMode_EI: + x_Ni[1] = round((0.5f / xf / df) * (FTM_FS)); + x_Ni[2] = 2 * x_Ni[1]; + #if HAS_Y_AXIS + y_Ni[1] = round((0.5f / yf / df) * (FTM_FS)); + y_Ni[2] = 2 * y_Ni[1]; + #endif + break; + case ftMotionMode_2HEI: + x_Ni[1] = round((0.5f / xf / df) * (FTM_FS)); + x_Ni[2] = 2 * x_Ni[1]; + x_Ni[3] = 3 * x_Ni[1]; + #if HAS_Y_AXIS + y_Ni[1] = round((0.5f / yf / df) * (FTM_FS)); + y_Ni[2] = 2 * y_Ni[1]; + y_Ni[3] = 3 * y_Ni[1]; + #endif + break; + case ftMotionMode_3HEI: + x_Ni[1] = round((0.5f / xf / df) * (FTM_FS)); + x_Ni[2] = 2 * x_Ni[1]; + x_Ni[3] = 3 * x_Ni[1]; + x_Ni[4] = 4 * x_Ni[1]; + #if HAS_Y_AXIS + y_Ni[1] = round((0.5f / yf / df) * (FTM_FS)); + y_Ni[2] = 2 * y_Ni[1]; + y_Ni[3] = 3 * y_Ni[1]; + y_Ni[4] = 4 * y_Ni[1]; + #endif + break; + case ftMotionMode_MZV: + x_Ni[1] = round((0.375f / xf / df) * (FTM_FS)); + x_Ni[2] = 2 * x_Ni[1]; + #if HAS_Y_AXIS + y_Ni[1] = round((0.375f / yf / df) * (FTM_FS)); + y_Ni[2] = 2 * y_Ni[1]; + #endif + break; + default: + for (uint32_t i = 0U; i < 5U; i++) { x_Ni[i] = 0; TERN_(HAS_Y_AXIS, y_Ni[i] = 0); } + } + } + +#endif // HAS_X_AXIS + +// Reset all trajectory processing variables. +void FxdTiCtrl::reset() { + + stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0; + + for (uint32_t i = 0U; i < (FTM_BATCH_SIZE); i++) { // Reset trajectory history + TERN_(HAS_X_AXIS, xd[i] = 0.0f); + TERN_(HAS_Y_AXIS, yd[i] = 0.0f); + TERN_(HAS_Z_AXIS, zd[i] = 0.0f); + TERN_(HAS_EXTRUDERS, ed[i] = 0.0f); + } + + blockProcRdy = blockProcRdy_z1 = blockProcDn = false; + batchRdy = batchRdyForInterp = false; + runoutEna = false; + + TERN_(HAS_X_AXIS, x_endPosn_prevBlock = 0.0f); + TERN_(HAS_Y_AXIS, y_endPosn_prevBlock = 0.0f); + TERN_(HAS_Z_AXIS, z_endPosn_prevBlock = 0.0f); + TERN_(HAS_EXTRUDERS, e_endPosn_prevBlock = 0.0f); + + makeVector_idx = makeVector_idx_z1 = 0; + makeVector_batchIdx = FTM_BATCH_SIZE; + + TERN_(HAS_X_AXIS, x_steps = 0); + TERN_(HAS_Y_AXIS, y_steps = 0); + TERN_(HAS_Z_AXIS, z_steps = 0); + TERN_(HAS_EXTRUDERS, e_steps = 0); + interpIdx = interpIdx_z1 = 0; + TERN_(HAS_X_AXIS, x_dirState = stepDirState_NOT_SET); + TERN_(HAS_Y_AXIS, y_dirState = stepDirState_NOT_SET); + TERN_(HAS_Z_AXIS, z_dirState = stepDirState_NOT_SET); + TERN_(HAS_EXTRUDERS, e_dirState = stepDirState_NOT_SET); + nextStepTicks = FTM_MIN_TICKS; + + #if HAS_X_AXIS + for (uint32_t i = 0U; i < (FTM_ZMAX); i++) { xd_zi[i] = 0.0f; TERN_(HAS_Y_AXIS, yd_zi[i] = 0.0f); } + xy_zi_idx = 0; + #endif + + TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f); +} + +// Private functions. +// Auxiliary function to get number of step commands in the buffer. +uint32_t FxdTiCtrl::stepperCmdBuffItems() { + const uint32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx; + return stepperCmdBuff_produceIdx < stepperCmdBuff_consumeIdx ? (FTM_STEPPERCMD_BUFF_SIZE) + udiff : udiff; +} + +// Initializes storage variables before startup. +void FxdTiCtrl::init() { + #if HAS_X_AXIS + updateShapingN(cfg_baseFreq[0] OPTARG(HAS_Y_AXIS, cfg_baseFreq[1])); + updateShapingA(FTM_SHAPING_ZETA, FTM_SHAPING_V_TOL); + #endif + reset(); // Precautionary. +} + +// Loads / converts block data from planner to fixed-time control variables. +void FxdTiCtrl::loadBlockData(block_t * const current_block) { + + const float totalLength = current_block->millimeters, + oneOverLength = 1.0f / totalLength; + + const axis_bits_t direction = current_block->direction_bits; + + #if HAS_X_AXIS + x_startPosn = x_endPosn_prevBlock; + float x_moveDist = current_block->steps.a / planner.settings.axis_steps_per_mm[X_AXIS]; + if (TEST(direction, X_AXIS)) x_moveDist *= -1.0f; + x_Ratio = x_moveDist * oneOverLength; + #endif + + #if HAS_Y_AXIS + y_startPosn = y_endPosn_prevBlock; + float y_moveDist = current_block->steps.b / planner.settings.axis_steps_per_mm[Y_AXIS]; + if (TEST(direction, Y_AXIS)) y_moveDist *= -1.0f; + y_Ratio = y_moveDist * oneOverLength; + #endif + + #if HAS_Z_AXIS + z_startPosn = z_endPosn_prevBlock; + float z_moveDist = current_block->steps.c / planner.settings.axis_steps_per_mm[Z_AXIS]; + if (TEST(direction, Z_AXIS)) z_moveDist *= -1.0f; + z_Ratio = z_moveDist * oneOverLength; + #endif + + #if HAS_EXTRUDERS + e_startPosn = e_endPosn_prevBlock; + float extrusion = current_block->steps.e / planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)]; + if (TEST(direction, E_AXIS_N(current_block->extruder))) extrusion *= -1.0f; + e_Ratio = extrusion * oneOverLength; + #endif + + const float spm = totalLength / current_block->step_event_count; // (steps/mm) Distance for each step + f_s = spm * current_block->initial_rate; // (steps/s) Start feedrate + const float f_e = spm * current_block->final_rate; // (steps/s) End feedrate + + const float a = current_block->acceleration, // (mm/s^2) Same magnitude for acceleration or deceleration + oneby2a = 1.0f / (2.0f * a), // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2 + oneby2d = -oneby2a; // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2 + const float fsSqByTwoA = sq(f_s) * oneby2a, // (mm) Distance to accelerate from start speed to nominal speed + feSqByTwoD = sq(f_e) * oneby2d; // (mm) Distance to decelerate from nominal speed to end speed + + float F_n = current_block->nominal_speed; // (mm/s) Speed we hope to achieve, if possible + const float fdiff = feSqByTwoD - fsSqByTwoA, // (mm) Coasting distance if nominal speed is reached + odiff = oneby2a - oneby2d, // (i.e., oneby2a * 2) (mm/s) Change in speed for one second of acceleration + ldiff = totalLength - fdiff; // (mm) Distance to travel if nominal speed is reached + float T2 = (1.0f / F_n) * (ldiff - odiff * sq(F_n)); // (s) Coasting duration after nominal speed reached + if (T2 < 0.0f) { + T2 = 0.0f; + F_n = SQRT(ldiff / odiff); // Clip by intersection if nominal speed can't be reached. + } + + const float T1 = (F_n - f_s) / a, // (s) Accel Time = difference in feedrate over acceleration + T3 = (F_n - f_e) / a; // (s) Decel Time = difference in feedrate over acceleration + + N1 = ceil(T1 * (FTM_FS)); // Accel datapoints based on Hz frequency + N2 = ceil(T2 * (FTM_FS)); // Coast + N3 = ceil(T3 * (FTM_FS)); // Decel + + const float T1_P = N1 * (FTM_TS), // (s) Accel datapoints x timestep resolution + T2_P = N2 * (FTM_TS), // (s) Coast + T3_P = N3 * (FTM_TS); // (s) Decel + + // Calculate the reachable feedrate at the end of the accel phase + // totalLength is the total distance to travel in mm + // f_s is the starting feedrate in mm/s + // f_e is the ending feedrate in mm/s + // T1_P is the time spent accelerating in seconds + // T2_P is the time spent coasting in seconds + // T3_P is the time spent decelerating in seconds + // f_s * T1_P is the distance traveled during the accel phase + // f_e * T3_P is the distance traveled during the decel phase + // + F_P = (2.0f * totalLength - f_s * T1_P - f_e * T3_P) / (T1_P + 2.0f * T2_P + T3_P); // (mm/s) Feedrate at the end of the accel phase + + // Calculate the acceleration and deceleration rates + accel_P = N1 ? ((F_P - f_s) / T1_P) : 0.0f; + + decel_P = (f_e - F_P) / T3_P; + + // Calculate the distance traveled during the accel phase + s_1e = f_s * T1_P + 0.5f * accel_P * sq(T1_P); + + // Calculate the distance traveled during the decel phase + s_2e = s_1e + F_P * T2_P; + + // One less than (Accel + Coasting + Decel) datapoints + max_intervals = N1 + N2 + N3 - 1U; + + TERN_(HAS_X_AXIS, x_endPosn_prevBlock += x_moveDist); + TERN_(HAS_Y_AXIS, y_endPosn_prevBlock += y_moveDist); + TERN_(HAS_Z_AXIS, z_endPosn_prevBlock += z_moveDist); + TERN_(HAS_EXTRUDERS, e_endPosn_prevBlock += extrusion); +} + +// Generate data points of the trajectory. +void FxdTiCtrl::makeVector() { + float accel_k = 0.0f; // (mm/s^2) Acceleration K factor + float tau = (makeVector_idx + 1) * (FTM_TS); // (s) Time since start of block + float dist = 0.0f; // (mm) Distance traveled + + if (makeVector_idx < N1) { + // Acceleration phase + dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase + accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase + } + else if (makeVector_idx >= N1 && makeVector_idx < (N1 + N2)) { + // Coasting phase + dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase + //accel_k = 0.0f; + } + else { + // Deceleration phase + const float tau_ = tau - (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase + dist = s_2e + F_P * tau_ + 0.5f * decel_P * sq(tau_); // (mm) Distance traveled for deceleration phase + accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase + } + + TERN_(HAS_X_AXIS, xd[makeVector_batchIdx] = x_startPosn + x_Ratio * dist); // (mm) X position for this datapoint + TERN_(HAS_Y_AXIS, yd[makeVector_batchIdx] = y_startPosn + y_Ratio * dist); // (mm) Y + TERN_(HAS_Z_AXIS, zd[makeVector_batchIdx] = z_startPosn + z_Ratio * dist); // (mm) Z + + #if HAS_EXTRUDERS + const float new_raw_z1 = e_startPosn + e_Ratio * dist; + if (cfg_linearAdvEna) { + float dedt_adj = (new_raw_z1 - e_raw_z1) * (FTM_FS); + if (e_Ratio > 0.0f) dedt_adj += accel_k * cfg_linearAdvK; + + e_advanced_z1 += dedt_adj * (FTM_TS); + ed[makeVector_batchIdx] = e_advanced_z1; + + e_raw_z1 = new_raw_z1; + } + else { + ed[makeVector_batchIdx] = new_raw_z1; + // Alternatively: coordArray_e[makeVector_batchIdx] = e_startDist + extrusion / (N1 + N2 + N3); + } + #endif + + // Update shaping parameters if needed. + #if HAS_Z_AXIS + static float zd_z1 = 0.0f; + #endif + switch (cfg_dynFreqMode) { + + #if HAS_Z_AXIS + case dynFreqMode_Z_BASED: + if (zd[makeVector_batchIdx] != zd_z1) { // Only update if Z changed. + const float xf = cfg_baseFreq[0] + cfg_dynFreqK[0] * zd[makeVector_batchIdx], + yf = cfg_baseFreq[1] + cfg_dynFreqK[1] * zd[makeVector_batchIdx]; + updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ), _MAX(yf, FTM_MIN_SHAPE_FREQ)); + zd_z1 = zd[makeVector_batchIdx]; + } + break; + #endif + + #if HAS_X_AXIS && HAS_EXTRUDERS + 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[0] + cfg_dynFreqK[0] * ed[makeVector_batchIdx] + OPTARG(HAS_Y_AXIS, cfg_baseFreq[1] + cfg_dynFreqK[1] * ed[makeVector_batchIdx]) ); + break; + #endif + + default: break; + } + + // Apply shaping if in mode. + #if HAS_X_AXIS + if (WITHIN(cfg_mode, 10U, 19U)) { + xd_zi[xy_zi_idx] = xd[makeVector_batchIdx]; + xd[makeVector_batchIdx] *= x_Ai[0]; + #if HAS_Y_AXIS + yd_zi[xy_zi_idx] = yd[makeVector_batchIdx]; + yd[makeVector_batchIdx] *= y_Ai[0]; + #endif + for (uint32_t i = 1U; i <= xy_max_i; i++) { + const uint32_t udiffx = xy_zi_idx - x_Ni[i]; + xd[makeVector_batchIdx] += x_Ai[i] * xd_zi[x_Ni[i] > xy_zi_idx ? (FTM_ZMAX) + udiffx : udiffx]; + #if HAS_Y_AXIS + const uint32_t udiffy = xy_zi_idx - y_Ni[i]; + yd[makeVector_batchIdx] += y_Ai[i] * yd_zi[y_Ni[i] > xy_zi_idx ? (FTM_ZMAX) + udiffy : udiffy]; + #endif + } + if (++xy_zi_idx == (FTM_ZMAX)) xy_zi_idx = 0; + } + #endif + + // Filled up the queue with regular and shaped steps + if (++makeVector_batchIdx == 2 * (FTM_BATCH_SIZE)) { + makeVector_batchIdx = FTM_BATCH_SIZE; + batchRdy = true; + } + + if (makeVector_idx == max_intervals) { + blockProcDn = true; + blockProcRdy = false; + makeVector_idx = 0; + } + else + makeVector_idx++; +} + +// Interpolates single data point to stepper commands. +void FxdTiCtrl::convertToSteps(const uint32_t idx) { + #if HAS_X_AXIS + int32_t x_err_P = 0; + #endif + #if HAS_Y_AXIS + int32_t y_err_P = 0; + #endif + #if HAS_Z_AXIS + int32_t z_err_P = 0; + #endif + #if HAS_EXTRUDERS + int32_t e_err_P = 0; + #endif + + //#define STEPS_ROUNDING + #if ENABLED(STEPS_ROUNDING) + #if HAS_X_AXIS + const float x_steps_tar = xm[idx] * planner.settings.axis_steps_per_mm[X_AXIS] + (xm[idx] < 0.0f ? -0.5f : 0.5f); // May be eliminated if guaranteed positive. + const int32_t x_delta = int32_t(x_steps_tar) - x_steps; + #endif + #if HAS_Y_AXIS + const float y_steps_tar = ym[idx] * planner.settings.axis_steps_per_mm[Y_AXIS] + (ym[idx] < 0.0f ? -0.5f : 0.5f); + const int32_t y_delta = int32_t(y_steps_tar) - y_steps; + #endif + #if HAS_Z_AXIS + const float z_steps_tar = zm[idx] * planner.settings.axis_steps_per_mm[Z_AXIS] + (zm[idx] < 0.0f ? -0.5f : 0.5f); + const int32_t z_delta = int32_t(z_steps_tar) - z_steps; + #endif + #if HAS_EXTRUDERS + const float e_steps_tar = em[idx] * planner.settings.axis_steps_per_mm[E_AXIS] + (em[idx] < 0.0f ? -0.5f : 0.5f); + const int32_t e_delta = int32_t(e_steps_tar) - e_steps; + #endif + #else + #if HAS_X_AXIS + const int32_t x_delta = int32_t(xm[idx] * planner.settings.axis_steps_per_mm[X_AXIS]) - x_steps; + #endif + #if HAS_Y_AXIS + const int32_t y_delta = int32_t(ym[idx] * planner.settings.axis_steps_per_mm[Y_AXIS]) - y_steps; + #endif + #if HAS_Z_AXIS + const int32_t z_delta = int32_t(zm[idx] * planner.settings.axis_steps_per_mm[Z_AXIS]) - z_steps; + #endif + #if HAS_EXTRUDERS + const int32_t e_delta = int32_t(em[idx] * planner.settings.axis_steps_per_mm[E_AXIS]) - e_steps; + #endif + #endif + + bool any_dirChange = (false + || TERN0(HAS_X_AXIS, (x_delta > 0 && x_dirState != stepDirState_POS) || (x_delta < 0 && x_dirState != stepDirState_NEG)) + || TERN0(HAS_Y_AXIS, (y_delta > 0 && y_dirState != stepDirState_POS) || (y_delta < 0 && y_dirState != stepDirState_NEG)) + || TERN0(HAS_Z_AXIS, (z_delta > 0 && z_dirState != stepDirState_POS) || (z_delta < 0 && z_dirState != stepDirState_NEG)) + || TERN0(HAS_EXTRUDERS, (e_delta > 0 && e_dirState != stepDirState_POS) || (e_delta < 0 && e_dirState != stepDirState_NEG)) + ); + + for (uint32_t i = 0U; i < (FTM_STEPS_PER_UNIT_TIME); i++) { + + // TODO: (?) Since the *delta variables will not change, + // the comparison may be done once before iterating at + // expense of storage and lines of code. + + bool anyStep = false; + + stepperCmdBuff[stepperCmdBuff_produceIdx] = 0; + + // Commands are written in the format: + // |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction| + #if HAS_X_AXIS + if (x_delta >= 0) { + if ((x_err_P + x_delta) < (FTM_CTS_COMPARE_VAL)) { + x_err_P += x_delta; + } + else { + x_steps++; + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_X) | _BV(FT_BIT_STEP_X); + x_err_P += x_delta - (FTM_STEPS_PER_UNIT_TIME); + anyStep = true; + } + } + else { + if ((x_err_P + x_delta) > -(FTM_CTS_COMPARE_VAL)) { + x_err_P += x_delta; + } + else { + x_steps--; + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_X); + x_err_P += x_delta + (FTM_STEPS_PER_UNIT_TIME); + anyStep = true; + } + } + #endif // HAS_X_AXIS + + #if HAS_Y_AXIS + if (y_delta >= 0) { + if ((y_err_P + y_delta) < (FTM_CTS_COMPARE_VAL)) { + y_err_P += y_delta; + } + else { + y_steps++; + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Y) | _BV(FT_BIT_STEP_Y); + y_err_P += y_delta - (FTM_STEPS_PER_UNIT_TIME); + anyStep = true; + } + } + else { + if ((y_err_P + y_delta) > -(FTM_CTS_COMPARE_VAL)) { + y_err_P += y_delta; + } + else { + y_steps--; + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_Y); + y_err_P += y_delta + (FTM_STEPS_PER_UNIT_TIME); + anyStep = true; + } + } + #endif // HAS_Y_AXIS + + #if HAS_Z_AXIS + if (z_delta >= 0) { + if ((z_err_P + z_delta) < (FTM_CTS_COMPARE_VAL)) { + z_err_P += z_delta; + } + else { + z_steps++; + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Z) | _BV(FT_BIT_STEP_Z); + z_err_P += z_delta - (FTM_STEPS_PER_UNIT_TIME); + anyStep = true; + } + } + else { + if ((z_err_P + z_delta) > -(FTM_CTS_COMPARE_VAL)) { + z_err_P += z_delta; + } + else { + z_steps--; + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_Z); + z_err_P += z_delta + (FTM_STEPS_PER_UNIT_TIME); + anyStep = true; + } + } + #endif // HAS_Z_AXIS + + #if HAS_EXTRUDERS + if (e_delta >= 0) { + if ((e_err_P + e_delta) < (FTM_CTS_COMPARE_VAL)) { + e_err_P += e_delta; + } + else { + e_steps++; + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_E) | _BV(FT_BIT_STEP_E); + e_err_P += e_delta - (FTM_STEPS_PER_UNIT_TIME); + anyStep = true; + } + } + else { + if ((e_err_P + e_delta) > -(FTM_CTS_COMPARE_VAL)) { + e_err_P += e_delta; + } + else { + e_steps--; + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_STEP_E); + e_err_P += e_delta + (FTM_STEPS_PER_UNIT_TIME); + anyStep = true; + } + } + #endif // HAS_EXTRUDERS + + if (!anyStep) { + nextStepTicks += (FTM_MIN_TICKS); + } + else { + stepperCmdBuff_StepRelativeTi[stepperCmdBuff_produceIdx] = nextStepTicks; + + const uint8_t dir_index = stepperCmdBuff_produceIdx >> 3, + dir_bit = stepperCmdBuff_produceIdx & 0x7; + if (any_dirChange) { + SBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit); + #if HAS_X_AXIS + if (x_delta > 0) { + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_X); + x_dirState = stepDirState_POS; + } + else { + x_dirState = stepDirState_NEG; + } + #endif + + #if HAS_Y_AXIS + if (y_delta > 0) { + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Y); + y_dirState = stepDirState_POS; + } + else { + y_dirState = stepDirState_NEG; + } + #endif + + #if HAS_Z_AXIS + if (z_delta > 0) { + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_Z); + z_dirState = stepDirState_POS; + } + else { + z_dirState = stepDirState_NEG; + } + #endif + + #if HAS_EXTRUDERS + if (e_delta > 0) { + stepperCmdBuff[stepperCmdBuff_produceIdx] |= _BV(FT_BIT_DIR_E); + e_dirState = stepDirState_POS; + } + else { + e_dirState = stepDirState_NEG; + } + #endif + + any_dirChange = false; + } + else { // ...no direction change. + CBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit); + } + + if (stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE) - 1) { + stepperCmdBuff_produceIdx = 0; + } + else { + stepperCmdBuff_produceIdx++; + } + + nextStepTicks = FTM_MIN_TICKS; + } + } // FTM_STEPS_PER_UNIT_TIME loop +} + +#endif // FT_MOTION diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h new file mode 100644 index 0000000000..2794608bf9 --- /dev/null +++ b/Marlin/src/module/ft_motion.h @@ -0,0 +1,170 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../inc/MarlinConfigPre.h" // Access the top level configurations. +#include "../module/planner.h" // Access block type from planner. + +#include "ft_types.h" + +#define FTM_STEPPERCMD_DIR_SIZE ((FTM_STEPPERCMD_BUFF_SIZE + 7) / 8) + +class FxdTiCtrl { + + public: + + // Public variables + static ftMotionMode_t cfg_mode; // Mode / active compensation mode configuration. + static bool cfg_linearAdvEna; // Linear advance enable configuration. + static float cfg_linearAdvK; // Linear advance gain. + static dynFreqMode_t cfg_dynFreqMode; // Dynamic frequency mode configuration. + + #if HAS_X_AXIS + static float cfg_baseFreq[1 + ENABLED(HAS_Y_AXIS)]; // Base frequency. [Hz] + static float cfg_dynFreqK[1 + ENABLED(HAS_Y_AXIS)]; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g] + #endif + + static uint8_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of stepper commands. + static hal_timer_t stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of the stepper command timing. + static uint8_t stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE]; // Buffer of whether DIR needs to be updated. + static uint32_t stepperCmdBuff_produceIdx, // Index of next stepper command write to the buffer. + stepperCmdBuff_consumeIdx; // Index of next stepper command read from the buffer. + + static bool sts_stepperBusy; // The stepper buffer has items and is in use. + + + // Public methods + static void startBlockProc(block_t * const current_block); // 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(const_float_t zeta=FTM_SHAPING_ZETA, const_float_t vtol=FTM_SHAPING_V_TOL); + + // 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), const_float_t zeta=FTM_SHAPING_ZETA); + #endif + + static void reset(); // Resets all states of the fixed time conversion to defaults. + + private: + + #if HAS_X_AXIS + static float xd[2 * (FTM_BATCH_SIZE)], xm[FTM_BATCH_SIZE]; + #endif + #if HAS_Y_AXIS + static float yd[2 * (FTM_BATCH_SIZE)], ym[FTM_BATCH_SIZE]; + #endif + #if HAS_Z_AXIS + static float zd[2 * (FTM_BATCH_SIZE)], zm[FTM_BATCH_SIZE]; + #endif + #if HAS_EXTRUDERS + static float ed[2 * (FTM_BATCH_SIZE)], em[FTM_BATCH_SIZE]; + #endif + + static block_t *current_block_cpy; + static bool blockProcRdy, blockProcRdy_z1, blockProcDn; + static bool batchRdy, batchRdyForInterp; + static bool runoutEna; + + // Trapezoid data variables. + #if HAS_X_AXIS + static float x_startPosn, x_endPosn_prevBlock, x_Ratio; + #endif + #if HAS_Y_AXIS + static float y_startPosn, y_endPosn_prevBlock, y_Ratio; + #endif + #if HAS_Z_AXIS + static float z_startPosn, z_endPosn_prevBlock, z_Ratio; + #endif + #if HAS_EXTRUDERS + static float e_startPosn, e_endPosn_prevBlock, e_Ratio; + #endif + static float accel_P, decel_P, + F_P, + f_s, + s_1e, + s_2e; + + static uint32_t N1, N2, N3; + static uint32_t max_intervals; + + // Make vector variables. + static uint32_t makeVector_idx, + makeVector_idx_z1, + makeVector_batchIdx; + + // Interpolation variables. + static uint32_t interpIdx, + interpIdx_z1; + #if HAS_X_AXIS + static int32_t x_steps; + static stepDirState_t x_dirState; + #endif + #if HAS_Y_AXIS + static int32_t y_steps; + static stepDirState_t y_dirState; + #endif + #if HAS_Z_AXIS + static int32_t z_steps; + static stepDirState_t z_dirState; + #endif + #if HAS_EXTRUDERS + static int32_t e_steps; + static stepDirState_t e_dirState; + #endif + + static hal_timer_t nextStepTicks; + + // Shaping variables. + #if HAS_X_AXIS + static uint32_t xy_zi_idx, xy_max_i; + static float xd_zi[FTM_ZMAX]; + static float x_Ai[5]; + static uint32_t x_Ni[5]; + #endif + #if HAS_Y_AXIS + static float yd_zi[FTM_ZMAX]; + static float y_Ai[5]; + static uint32_t y_Ni[5]; + #endif + + // Linear advance variables. + #if HAS_EXTRUDERS + static float e_raw_z1, e_advanced_z1; + #endif + + // Private methods + static uint32_t stepperCmdBuffItems(); + static void init(); + static void loadBlockData(block_t * const current_block); + static void makeVector(); + static void convertToSteps(const uint32_t idx); + +}; // class fxdTiCtrl + +extern FxdTiCtrl fxdTiCtrl; diff --git a/Marlin/src/module/ft_types.h b/Marlin/src/module/ft_types.h new file mode 100644 index 0000000000..613e177a39 --- /dev/null +++ b/Marlin/src/module/ft_types.h @@ -0,0 +1,59 @@ +/** + * Marlin 3D Printer Firmware + * Copyright (c) 2023 MarlinFirmware [https://github.com/MarlinFirmware/Marlin] + * + * Based on Sprinter and grbl. + * Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + */ +#pragma once + +#include "../core/types.h" + +typedef enum FXDTICtrlMode : uint8_t { + ftMotionMode_DISABLED = 0U, + ftMotionMode_ENABLED = 1U, + ftMotionMode_ULENDO_FBS = 2U, + ftMotionMode_ZV = 10U, + ftMotionMode_ZVD = 11U, + ftMotionMode_EI = 12U, + ftMotionMode_2HEI = 13U, + ftMotionMode_3HEI = 14U, + ftMotionMode_MZV = 15U, + ftMotionMode_DISCTF = 20U +} ftMotionMode_t; + +enum dynFreqMode_t : uint8_t { + dynFreqMode_DISABLED = 0U, + dynFreqMode_Z_BASED = 1U, + dynFreqMode_MASS_BASED = 2U +}; + +enum stepDirState_t { + stepDirState_NOT_SET = 0U, + stepDirState_POS = 1U, + stepDirState_NEG = 2U +}; + +enum { + FT_BIT_DIR_E, FT_BIT_STEP_E, + FT_BIT_DIR_Z, FT_BIT_STEP_Z, + FT_BIT_DIR_Y, FT_BIT_STEP_Y, + FT_BIT_DIR_X, FT_BIT_STEP_X, + FT_BIT_COUNT +}; + +typedef bits_t(FT_BIT_COUNT) ft_command_t; diff --git a/Marlin/src/module/planner.cpp b/Marlin/src/module/planner.cpp index 552d212e2e..dadb22fbde 100644 --- a/Marlin/src/module/planner.cpp +++ b/Marlin/src/module/planner.cpp @@ -69,6 +69,9 @@ #include "stepper.h" #include "motion.h" #include "temperature.h" +#if ENABLED(FT_MOTION) + #include "ft_motion.h" +#endif #include "../lcd/marlinui.h" #include "../gcode/parser.h" @@ -112,7 +115,8 @@ // Delay for delivery of first block to the stepper ISR, if the queue contains 2 or // fewer movements. The delay is measured in milliseconds, and must be less than 250ms -#define BLOCK_DELAY_FOR_1ST_MOVE 100 +#define BLOCK_DELAY_NONE 0U +#define BLOCK_DELAY_FOR_1ST_MOVE 100U Planner planner; @@ -127,7 +131,7 @@ volatile uint8_t Planner::block_buffer_head, // Index of the next block to be Planner::block_buffer_planned, // Index of the optimally planned block Planner::block_buffer_tail; // Index of the busy block, if any uint16_t Planner::cleaning_buffer_counter; // A counter to disable queuing of blocks -uint8_t Planner::delay_before_delivering; // This counter delays delivery of blocks when queue becomes empty to allow the opportunity of merging blocks +uint8_t Planner::delay_before_delivering; // Delay block delivery so initial blocks in an empty queue may merge planner_settings_t Planner::settings; // Initialized by settings.load() @@ -225,6 +229,10 @@ float Planner::previous_nominal_speed; int32_t Planner::xy_freq_min_interval_us = LROUND(1000000.0f / (XY_FREQUENCY_LIMIT)); #endif +#if ENABLED(FT_MOTION) + bool Planner::fxdTiCtrl_busy = false; +#endif + #if ENABLED(LIN_ADVANCE) float Planner::extruder_advance_K[DISTINCT_E]; // Initialized by settings.load() #endif @@ -1683,7 +1691,8 @@ 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 = BLOCK_DELAY_FOR_1ST_MOVE; + + delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; TERN_(HAS_WIRED_LCD, clear_block_buffer_runtime()); // Clear the accumulated runtime @@ -1729,6 +1738,7 @@ bool Planner::busy() { return (has_blocks_queued() || cleaning_buffer_counter || TERN0(EXTERNAL_CLOSED_LOOP_CONTROLLER, CLOSED_LOOP_WAITING()) || TERN0(HAS_ZV_SHAPING, stepper.input_shaping_busy()) + || TERN0(FT_MOTION, fxdTiCtrl_busy) ); } @@ -1841,7 +1851,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 = BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } // Move buffer head @@ -2945,7 +2955,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 = BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } block_buffer_head = next_buffer_head; @@ -3243,7 +3253,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 = BLOCK_DELAY_FOR_1ST_MOVE; + delay_before_delivering = TERN_(FT_MOTION, fxdTiCtrl.cfg_mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE; } // Move buffer head diff --git a/Marlin/src/module/planner.h b/Marlin/src/module/planner.h index ccf6ba08d3..e2d1d6739c 100644 --- a/Marlin/src/module/planner.h +++ b/Marlin/src/module/planner.h @@ -512,6 +512,10 @@ class Planner { } #endif + #if ENABLED(FT_MOTION) + static bool fxdTiCtrl_busy; + #endif + private: /** diff --git a/Marlin/src/module/stepper.cpp b/Marlin/src/module/stepper.cpp index 34a5ca28a3..1062a778d8 100644 --- a/Marlin/src/module/stepper.cpp +++ b/Marlin/src/module/stepper.cpp @@ -91,6 +91,10 @@ Stepper stepper; // Singleton #include "planner.h" #include "motion.h" +#if ENABLED(FT_MOTION) + #include "ft_motion.h" +#endif + #include "../lcd/marlinui.h" #include "../gcode/queue.h" #include "../sd/cardreader.h" @@ -1488,63 +1492,133 @@ void Stepper::isr() { // Limit the amount of iterations uint8_t max_loops = 10; + #if ENABLED(FT_MOTION) + static bool fxdTiCtrl_stepCmdRdy = false; // Indicates a step command was loaded from the + // buffers and is ready to be output. + static bool fxdTiCtrl_applyDir = false; // Indicates the DIR output should be set. + static ft_command_t fxdTiCtrl_stepCmd = 0U; // Storage for the step command to be output. + static uint32_t fxdTiCtrl_nextAuxISR = 0U; // Storage for the next ISR of the auxilliary tasks. + #endif + // We need this variable here to be able to use it in the following loop hal_timer_t min_ticks; do { // Enable ISRs to reduce USART processing latency hal.isr_on(); - TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed + hal_timer_t interval; - if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses + #if ENABLED(FT_MOTION) - #if ENABLED(LIN_ADVANCE) - if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses - advance_isr(); - nextAdvanceISR = la_interval; + // NOTE STEPPER_TIMER_RATE is equal to 2000000, not what VSCode shows + const bool using_fxtictrl = fxdTiCtrl.cfg_mode; + if (using_fxtictrl) { + if (!nextMainISR) { + if (abort_current_block) { + fxdTiCtrl_stepCmdRdy = false; // If a command was ready, cancel it. + fxdTiCtrl.sts_stepperBusy = false; // Set busy false to allow a reset. + nextMainISR = 0.01f * (STEPPER_TIMER_RATE); // Come back in 10 msec. + } + else { // !(abort_current_block) + if (fxdTiCtrl_stepCmdRdy) { + fxdTiCtrl_stepper(fxdTiCtrl_applyDir, fxdTiCtrl_stepCmd); + fxdTiCtrl_stepCmdRdy = false; + } + // Check if there is data in the buffers. + if (fxdTiCtrl.stepperCmdBuff_produceIdx != fxdTiCtrl.stepperCmdBuff_consumeIdx) { + + fxdTiCtrl.sts_stepperBusy = true; + + // "Pop" one command from the command buffer. + fxdTiCtrl_stepCmd = fxdTiCtrl.stepperCmdBuff[fxdTiCtrl.stepperCmdBuff_consumeIdx]; + const uint8_t dir_index = fxdTiCtrl.stepperCmdBuff_consumeIdx >> 3, + dir_bit = fxdTiCtrl.stepperCmdBuff_consumeIdx & 0x7; + fxdTiCtrl_applyDir = TEST(fxdTiCtrl.stepperCmdBuff_ApplyDir[dir_index], dir_bit); + nextMainISR = fxdTiCtrl.stepperCmdBuff_StepRelativeTi[fxdTiCtrl.stepperCmdBuff_consumeIdx]; + fxdTiCtrl_stepCmdRdy = true; + + if (++fxdTiCtrl.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE)) + fxdTiCtrl.stepperCmdBuff_consumeIdx = 0; + + } + else { // Buffer empty. + fxdTiCtrl.sts_stepperBusy = false; + nextMainISR = 0.01f * (STEPPER_TIMER_RATE); // Come back in 10 msec. + } + } // !(abort_current_block) + } // if (!nextMainISR) + + // Define 2.5 msec task for auxilliary functions. + if (!fxdTiCtrl_nextAuxISR) { + endstops.update(); + TERN_(INTEGRATED_BABYSTEPPING, if (babystep.has_steps()) babystepping_isr()); + fxdTiCtrl_refreshAxisDidMove(); + fxdTiCtrl_nextAuxISR = 0.0025f * (STEPPER_TIMER_RATE); + } + + interval = _MIN(nextMainISR, fxdTiCtrl_nextAuxISR); + nextMainISR -= interval; + fxdTiCtrl_nextAuxISR -= interval; } - else if (nextAdvanceISR == LA_ADV_NEVER) // Start LA steps if necessary - nextAdvanceISR = la_interval; + + #else + + constexpr bool using_fxtictrl = false; + #endif - #if ENABLED(INTEGRATED_BABYSTEPPING) - const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses - if (is_babystep) nextBabystepISR = babystepping_isr(); - #endif + if (!using_fxtictrl) { - // ^== Time critical. NOTHING besides pulse generation should be above here!!! + TERN_(HAS_ZV_SHAPING, shaping_isr()); // Do Shaper stepping, if needed - if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block + if (!nextMainISR) pulse_phase_isr(); // 0 = Do coordinated axes Stepper pulses - #if ENABLED(INTEGRATED_BABYSTEPPING) - if (is_babystep) // Avoid ANY stepping too soon after baby-stepping - NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step + #if ENABLED(LIN_ADVANCE) + if (!nextAdvanceISR) { // 0 = Do Linear Advance E Stepper pulses + advance_isr(); + nextAdvanceISR = la_interval; + } + else if (nextAdvanceISR == LA_ADV_NEVER) // Start LA steps if necessary + nextAdvanceISR = la_interval; + #endif - if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping - NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping - #endif + #if ENABLED(INTEGRATED_BABYSTEPPING) + const bool is_babystep = (nextBabystepISR == 0); // 0 = Do Babystepping (XY)Z pulses + if (is_babystep) nextBabystepISR = babystepping_isr(); + #endif - // Get the interval to the next ISR call - const hal_timer_t interval = _MIN( - hal_timer_t(HAL_TIMER_TYPE_MAX), // Come back in a very long time - nextMainISR // Time until the next Pulse / Block phase - OPTARG(INPUT_SHAPING_X, ShapingQueue::peek_x()) // Time until next input shaping echo for X - OPTARG(INPUT_SHAPING_Y, ShapingQueue::peek_y()) // Time until next input shaping echo for Y - OPTARG(LIN_ADVANCE, nextAdvanceISR) // Come back early for Linear Advance? - OPTARG(INTEGRATED_BABYSTEPPING, nextBabystepISR) // Come back early for Babystepping? - ); + // ^== Time critical. NOTHING besides pulse generation should be above here!!! - // - // Compute remaining time for each ISR phase - // NEVER : The phase is idle - // Zero : The phase will occur on the next ISR call - // Non-zero : The phase will occur on a future ISR call - // + if (!nextMainISR) nextMainISR = block_phase_isr(); // Manage acc/deceleration, get next block - nextMainISR -= interval; - TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval)); - TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval); - TERN_(INTEGRATED_BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval); + #if ENABLED(INTEGRATED_BABYSTEPPING) + if (is_babystep) // Avoid ANY stepping too soon after baby-stepping + NOLESS(nextMainISR, (BABYSTEP_TICKS) / 8); // FULL STOP for 125µs after a baby-step + + if (nextBabystepISR != BABYSTEP_NEVER) // Avoid baby-stepping too close to axis Stepping + NOLESS(nextBabystepISR, nextMainISR / 2); // TODO: Only look at axes enabled for baby-stepping + #endif + + // Get the interval to the next ISR call + interval = _MIN(nextMainISR, uint32_t(HAL_TIMER_TYPE_MAX)); // Time until the next Pulse / Block phase + TERN_(INPUT_SHAPING_X, NOMORE(interval, ShapingQueue::peek_x())); // Time until next input shaping echo for X + TERN_(INPUT_SHAPING_Y, NOMORE(interval, ShapingQueue::peek_y())); // Time until next input shaping echo for Y + TERN_(LIN_ADVANCE, NOMORE(interval, nextAdvanceISR)); // Come back early for Linear Advance? + TERN_(INTEGRATED_BABYSTEPPING, NOMORE(interval, nextBabystepISR)); // Come back early for Babystepping? + + // + // Compute remaining time for each ISR phase + // NEVER : The phase is idle + // Zero : The phase will occur on the next ISR call + // Non-zero : The phase will occur on a future ISR call + // + + nextMainISR -= interval; + TERN_(HAS_ZV_SHAPING, ShapingQueue::decrement_delays(interval)); + TERN_(LIN_ADVANCE, if (nextAdvanceISR != LA_ADV_NEVER) nextAdvanceISR -= interval); + TERN_(INTEGRATED_BABYSTEPPING, if (nextBabystepISR != BABYSTEP_NEVER) nextBabystepISR -= interval); + + } // standard motion control /** * This needs to avoid a race-condition caused by interleaving @@ -1978,7 +2052,7 @@ void Stepper::pulse_phase_isr() { #if ENABLED(MIXING_EXTRUDER) if (step_needed.e) { - count_position[E_AXIS] += count_direction[E_AXIS]; + count_position.e += count_direction.e; E_STEP_WRITE(mixer.get_next_stepper(), STEP_STATE_E); } #elif HAS_E0_STEP @@ -3381,6 +3455,127 @@ void Stepper::report_positions() { report_a_position(pos); } +#if ENABLED(FT_MOTION) + + // Set stepper I/O for fixed time controller. + void Stepper::fxdTiCtrl_stepper(const bool applyDir, const ft_command_t command) { + + USING_TIMED_PULSE(); + + #if HAS_Z_AXIS + // Z is handled differently to update the stepper + // counts (needed by Marlin for bed level probing). + const bool z_dir = !TEST(command, FT_BIT_DIR_Z), + z_step = TEST(command, FT_BIT_STEP_Z); + #endif + + if (applyDir) { + X_DIR_WRITE(TEST(command, FT_BIT_DIR_X)); + TERN_(HAS_Y_AXIS, Y_DIR_WRITE(TEST(command, FT_BIT_DIR_Y))); + TERN_(HAS_Z_AXIS, Z_DIR_WRITE(z_dir)); + TERN_(HAS_EXTRUDERS, E0_DIR_WRITE(TEST(command, FT_BIT_DIR_E))); + DIR_WAIT_AFTER(); + } + + X_STEP_WRITE(TEST(command, FT_BIT_STEP_X)); + TERN_(HAS_Y_AXIS, Y_STEP_WRITE(TEST(command, FT_BIT_STEP_Y))); + TERN_(HAS_Z_AXIS, Z_STEP_WRITE(z_step)); + TERN_(HAS_EXTRUDERS, E0_STEP_WRITE(TEST(command, FT_BIT_STEP_E))); + + START_TIMED_PULSE(); + + #if HAS_Z_AXIS + // Update step counts + if (z_step) count_position.z += z_dir ? -1 : 1; + #endif + + AWAIT_HIGH_PULSE(); + + X_STEP_WRITE(0); + TERN_(HAS_Y_AXIS, Y_STEP_WRITE(0)); + TERN_(HAS_Z_AXIS, Z_STEP_WRITE(0)); + TERN_(HAS_EXTRUDERS, E0_STEP_WRITE(0)); + + } // Stepper::fxdTiCtrl_stepper + + void Stepper::fxdTiCtrl_BlockQueueUpdate() { + + if (current_block) { + // If the current block is not done processing, return right away + if (!fxdTiCtrl.getBlockProcDn()) return; + + axis_did_move = 0; + current_block = nullptr; + discard_current_block(); + } + + if (!current_block) { // No current block + + // Check the buffer for a new block + current_block = planner.get_current_block(); + + if (current_block) { + // Sync block? Sync the stepper counts and return + while (current_block->is_sync()) { + if (!(current_block->is_fan_sync() || current_block->is_pwr_sync())) _set_position(current_block->position); + discard_current_block(); + + // Try to get a new block + if (!(current_block = planner.get_current_block())) + return; // No more queued movements!image.png + } + + // this is needed by motor_direction() and subsequently bed leveling (somehow) + // update it here, even though it will may be out of sync with step commands + last_direction_bits = current_block->direction_bits; + + fxdTiCtrl.startBlockProc(current_block); + + } + else { + fxdTiCtrl.runoutBlock(); + return; // No queued blocks + } + + } // if (!current_block) + + } // Stepper::fxdTiCtrl_BlockQueueUpdate() + + // Debounces the axis move indication to account for potential + // delay between the block information and the stepper commands + void Stepper::fxdTiCtrl_refreshAxisDidMove() { + + // Set the debounce time in seconds. + #define AXIS_DID_MOVE_DEB 5 // TODO: The debounce time should be calculated if possible, + // or the set conditions should be changed from the block to + // the motion trajectory or motor commands. + + uint8_t axis_bits = 0U; + + static uint32_t a_debounce = 0U; + if (!!current_block->steps.a) a_debounce = (AXIS_DID_MOVE_DEB) * 400; // divide by 0.0025f + if (a_debounce) { SBI(axis_bits, A_AXIS); a_debounce--; } + #if HAS_Y_AXIS + static uint32_t b_debounce = 0U; + if (!!current_block->steps.b) b_debounce = (AXIS_DID_MOVE_DEB) * 400; + if (b_debounce) { SBI(axis_bits, B_AXIS); b_debounce--; } + #endif + #if HAS_Z_AXIS + static uint32_t c_debounce = 0U; + if (!!current_block->steps.c) c_debounce = (AXIS_DID_MOVE_DEB) * 400; + if (c_debounce) { SBI(axis_bits, C_AXIS); c_debounce--; } + #endif + #if HAS_EXTRUDERS + static uint32_t e_debounce = 0U; + if (!!current_block->steps.e) e_debounce = (AXIS_DID_MOVE_DEB) * 400; + if (e_debounce) { SBI(axis_bits, E_AXIS); e_debounce--; } + #endif + + axis_did_move = axis_bits; + } + +#endif // FT_MOTION + #if ENABLED(BABYSTEPPING) #define _ENABLE_AXIS(A) enable_axis(_AXIS(A)) diff --git a/Marlin/src/module/stepper.h b/Marlin/src/module/stepper.h index 4adbb01991..4c54a5dbf9 100644 --- a/Marlin/src/module/stepper.h +++ b/Marlin/src/module/stepper.h @@ -49,6 +49,10 @@ #include "stepper/speed_lookuptable.h" #endif +#if ENABLED(FT_MOTION) + #include "ft_types.h" +#endif + // // Estimate the amount of time the Stepper ISR will take to execute // @@ -470,6 +474,7 @@ constexpr ena_mask_t enable_overlap[] = { // class Stepper { friend class Max7219; + friend class FxdTiCtrl; friend void stepperTask(void *); public: @@ -817,6 +822,11 @@ class Stepper { set_directions(); } + #if ENABLED(FT_MOTION) + // Manage the planner + static void fxdTiCtrl_BlockQueueUpdate(); + #endif + #if HAS_ZV_SHAPING static void set_shaping_damping_ratio(const AxisEnum axis, const_float_t zeta); static float get_shaping_damping_ratio(const AxisEnum axis); @@ -848,6 +858,11 @@ class Stepper { static void microstep_init(); #endif + #if ENABLED(FT_MOTION) + static void fxdTiCtrl_stepper(const bool applyDir, const ft_command_t command); + static void fxdTiCtrl_refreshAxisDidMove(); + #endif + }; extern Stepper stepper; diff --git a/buildroot/tests/STM32F103RC_btt b/buildroot/tests/STM32F103RC_btt index 16419cbfa2..95a18c615f 100755 --- a/buildroot/tests/STM32F103RC_btt +++ b/buildroot/tests/STM32F103RC_btt @@ -12,8 +12,8 @@ set -e restore_configs opt_set MOTHERBOARD BOARD_BTT_SKR_MINI_E3_V1_0 SERIAL_PORT 1 SERIAL_PORT_2 -1 \ X_DRIVER_TYPE TMC2209 Y_DRIVER_TYPE TMC2209 Z_DRIVER_TYPE TMC2209 E0_DRIVER_TYPE TMC2209 -opt_enable PINS_DEBUGGING Z_IDLE_HEIGHT -exec_test $1 $2 "BigTreeTech SKR Mini E3 1.0 - Basic Config with TMC2209 HW Serial" "$3" +opt_enable PINS_DEBUGGING Z_IDLE_HEIGHT FT_MOTION +exec_test $1 $2 "BigTreeTech SKR Mini E3 1.0 - TMC2209 HW Serial, FT_MOTION" "$3" # clean up restore_configs diff --git a/ini/features.ini b/ini/features.ini index 4e378a8eaf..d8ba74f2db 100644 --- a/ini/features.ini +++ b/ini/features.ini @@ -186,6 +186,7 @@ AIR_EVACUATION = src_filter=+ SERVO_DETACH_GCODE = src_filter=+ HAS_DUPLICATION_MODE = src_filter=+ +FT_MOTION = src_filter=+ + LIN_ADVANCE = src_filter=+ PHOTO_GCODE = src_filter=+ CONTROLLER_FAN_EDITABLE = src_filter=+ diff --git a/platformio.ini b/platformio.ini index 9fbc589d2e..3478dcc1fb 100644 --- a/platformio.ini +++ b/platformio.ini @@ -192,6 +192,7 @@ default_src_filter = + - - + - - - + - - - - -