Skip to content

Commit

Permalink
🧑‍💻 FT Motion: Individual axis shaping, new buffer management (#26848)
Browse files Browse the repository at this point in the history 
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
  • Loading branch information
@narno2202 @thinkyhead
narno2202 and thinkyhead authored Jul 15, 2024
1 parent 20a704b commit f0bc427
Show file tree
Hide file tree
Showing 13 changed files with 502 additions and 652 deletions.
14 changes: 6 additions & 8 deletions Marlin/Configuration_adv.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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
Expand Down
2 changes: 1 addition & 1 deletion Marlin/src/HAL/ESP32/i2s.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {

Expand Down
182 changes: 110 additions & 72 deletions Marlin/src/gcode/feature/ft_motion/M493.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,38 +28,59 @@
#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),
g_based = TERN0(HAS_DYNAMIC_FREQ_G, ftMotion.cfg.dynFreqMode == dynFreqMode_MASS_BASED),
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) {
Expand All @@ -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"));
Expand All @@ -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"));
Expand All @@ -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
Expand All @@ -133,18 +154,21 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) {
/**
* M493: Set Fixed-time Motion Control parameters
*
* S<mode> Set the motion / shaping mode. Shaping requires an X axis, at the minimum.
* S<bool> 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<mode> 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<bool> Enable (1) or Disable (0) Linear Advance pressure control
*
Expand All @@ -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 (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 (parser.seen('S')) {
const bool active = parser.value_bool();

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.
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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.");
Expand All @@ -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.
Expand All @@ -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.
Expand All @@ -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.");
Expand All @@ -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
Expand All @@ -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.
Expand All @@ -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();
Expand Down
9 changes: 9 additions & 0 deletions Marlin/src/inc/SanityCheck.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion Marlin/src/lcd/language/language_en.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down
Loading

0 comments on commit f0bc427

Please sign in to comment.