Skip to content

Commit

Permalink
🐛⚡️ FT_MOTION improvements (#26074)
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 and thinkyhead authored Dec 20, 2023
1 parent 0ede16c commit d9f3d5f
Show file tree
Hide file tree
Showing 16 changed files with 657 additions and 645 deletions.
84 changes: 52 additions & 32 deletions Marlin/Configuration_adv.h
Original file line number Diff line number Diff line change
Expand Up @@ -1121,42 +1121,62 @@
#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_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.
#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_X 0.1f // Zeta used by input shapers for X axis
#define FTM_SHAPING_ZETA_Y 0.1f // Zeta used by input shapers for Y axis

#define FTM_SHAPING_V_TOL_X 0.05f // Vibration tolerance used by EI input shapers for X axis
#define FTM_SHAPING_V_TOL_Y 0.05f // Vibration tolerance used by EI input shapers for Y axis

//#define FT_MOTION_MENU // Provide a MarlinUI menu to set M493 parameters

/**
* Advanced configuration
*/
#define FTM_BATCH_SIZE 100 // Batch size for trajectory generation;
#define FTM_WINDOW_SIZE 200 // Window size for trajectory generation.
#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.

//#define FT_MOTION_MENU // Provide a MarlinUI menu to set M493 parameters.
#define FTM_UNIFIED_BWS // DON'T DISABLE unless you use Ulendo FBS (not implemented)
#if ENABLED(FTM_UNIFIED_BWS)
#define FTM_BW_SIZE 100 // Unified Window and Batch size with a ratio of 2
#else
#define FTM_WINDOW_SIZE 200 // Custom Window size for trajectory generation needed by Ulendo FBS
#define FTM_BATCH_SIZE 100 // Custom Batch size for trajectory generation needed by Ulendo FBS
#endif

#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
#define FTM_STEPPERCMD_BUFF_SIZE 6000
#endif

#define FTM_STEPS_PER_UNIT_TIME (FTM_STEPPER_FS / FTM_FS) // Interpolated stepper commands per unit time
#define FTM_CTS_COMPARE_VAL (FTM_STEPS_PER_UNIT_TIME / 2) // Comparison value used in interpolation algorithm
#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_RATIO (FTM_FS / FTM_MIN_SHAPE_FREQ) // Factor for use in FTM_ZMAX. DON'T CHANGE.
#define FTM_ZMAX (FTM_RATIO * 2) // Maximum delays for shaping functions (even numbers only!)
// Calculate as:
// ZV : FTM_RATIO / 2
// ZVD, MZV : FTM_RATIO
// 2HEI : FTM_RATIO * 3 / 2
// 3HEI : FTM_RATIO * 2
#endif

/**
Expand Down
4 changes: 2 additions & 2 deletions Marlin/src/gcode/bedlevel/G35.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,9 +57,9 @@
* 41 - Counter-Clockwise M4
* 50 - Clockwise M5
* 51 - Counter-Clockwise M5
*
*/
**/
void GcodeSuite::G35() {

DEBUG_SECTION(log_G35, "G35", DEBUGGING(LEVELING));

if (DEBUGGING(LEVELING)) log_machine_info();
Expand Down
1 change: 1 addition & 0 deletions Marlin/src/gcode/bedlevel/abl/G29.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -225,6 +225,7 @@ class G29_State {
* There's no extra effect if you have a fixed Z probe.
*/
G29_TYPE GcodeSuite::G29() {

DEBUG_SECTION(log_G29, "G29", DEBUGGING(LEVELING));

// Leveling state is persistent when done manually with multiple G29 commands
Expand Down
1 change: 1 addition & 0 deletions Marlin/src/gcode/bedlevel/mbl/G29.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@ inline void echo_not_entered(const char c) { SERIAL_CHAR(c); SERIAL_ECHOLNPGM("
* S5 Reset and disable mesh
*/
void GcodeSuite::G29() {

DEBUG_SECTION(log_G29, "G29", true);

// G29 Q is also available if debugging
Expand Down
1 change: 1 addition & 0 deletions Marlin/src/gcode/calibrate/G34_M422.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,7 @@
* R Flag to recalculate points based on current probe offsets
*/
void GcodeSuite::G34() {

DEBUG_SECTION(log_G34, "G34", DEBUGGING(LEVELING));
if (DEBUGGING(LEVELING)) log_machine_info();

Expand Down
107 changes: 84 additions & 23 deletions Marlin/src/gcode/feature/ft_motion/M493.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@

#include "../../gcode.h"
#include "../../../module/ft_motion.h"
#include "../../../module/stepper.h"

void say_shaping() {
// FT Enabled
Expand All @@ -39,6 +40,8 @@ void say_shaping() {
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;
Expand Down Expand Up @@ -155,20 +158,19 @@ void GcodeSuite::M493() {

if (!parser.seen_any())
flag.report_h = true;
else
planner.synchronize();

// Parse 'S' mode parameter.
if (parser.seenval('S')) {
const ftMotionMode_t oldmm = ftMotion.cfg.mode,
newmm = (ftMotionMode_t)parser.value_byte();
const ftMotionMode_t newmm = (ftMotionMode_t)parser.value_byte();

if (newmm != oldmm) {
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:
Expand All @@ -177,11 +179,10 @@ void GcodeSuite::M493() {
//case ftMotionMode_DISCTF:
flag.update_n = flag.update_a = true;
#endif
case ftMotionMode_DISABLED:
case ftMotionMode_DISABLED: flag.reset_ft = true;
case ftMotionMode_ENABLED:
ftMotion.cfg.mode = newmm;
flag.report_h = true;
if (oldmm == ftMotionMode_DISABLED) flag.reset_ft = true;
break;
}
}
Expand All @@ -193,6 +194,7 @@ void GcodeSuite::M493() {
if (parser.seen('P')) {
const bool val = parser.value_bool();
ftMotion.cfg.linearAdvEna = val;
flag.report_h = true;
SERIAL_ECHO_TERNARY(val, "Linear Advance ", "en", "dis", "abled.\n");
}

Expand All @@ -216,22 +218,16 @@ void GcodeSuite::M493() {
if (ftMotion.cfg.modeHasShaper()) {
const dynFreqMode_t val = dynFreqMode_t(parser.value_byte());
switch (val) {
case dynFreqMode_DISABLED:
ftMotion.cfg.dynFreqMode = val;
flag.report_h = true;
break;
#if HAS_DYNAMIC_FREQ_MM
case dynFreqMode_Z_BASED:
ftMotion.cfg.dynFreqMode = val;
flag.report_h = true;
break;
#endif
#if HAS_DYNAMIC_FREQ_G
case dynFreqMode_MASS_BASED:
ftMotion.cfg.dynFreqMode = val;
flag.report_h = true;
break;
#endif
case dynFreqMode_DISABLED:
ftMotion.cfg.dynFreqMode = val;
flag.report_h = true;
break;
default:
SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value.");
break;
Expand Down Expand Up @@ -279,6 +275,36 @@ void GcodeSuite::M493() {
}
#endif

// Parse zeta parameter (X axis).
if (parser.seenval('I')) {
const float val = parser.value_float();
if (ftMotion.cfg.modeHasShaper()) {
if (WITHIN(val, 0.01f, 1.0f)) {
ftMotion.cfg.zeta[0] = val;
flag.update_n = flag.update_a = true;
}
else
SERIAL_ECHOLNPGM("Invalid X zeta [", AS_CHAR('I'), "] value."); // Zeta out of range.
}
else
SERIAL_ECHOLNPGM("Wrong mode for zeta parameter.");
}

// 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 (WITHIN(val, 0.00f, 1.0f)) {
ftMotion.cfg.vtol[0] = val;
flag.update_a = true;
}
else
SERIAL_ECHOLNPGM("Invalid X vtol [", AS_CHAR('Q'), "] value."); // VTol out of range.
}
else
SERIAL_ECHOLNPGM("Wrong mode for vtol parameter.");
}

#endif // HAS_X_AXIS

#if HAS_Y_AXIS
Expand Down Expand Up @@ -310,15 +336,50 @@ void GcodeSuite::M493() {
}
#endif

// Parse zeta parameter (Y axis).
if (parser.seenval('J')) {
const float val = parser.value_float();
if (ftMotion.cfg.modeHasShaper()) {
if (WITHIN(val, 0.01f, 1.0f)) {
ftMotion.cfg.zeta[1] = val;
flag.update_n = flag.update_a = true;
}
else
SERIAL_ECHOLNPGM("Invalid Y zeta [", AS_CHAR('J'), "] value."); // Zeta Out of range
}
else
SERIAL_ECHOLNPGM("Wrong mode for zeta parameter.");
}

// 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 (WITHIN(val, 0.00f, 1.0f)) {
ftMotion.cfg.vtol[1] = val;
flag.update_a = true;
}
else
SERIAL_ECHOLNPGM("Invalid Y vtol [", AS_CHAR('R'), "] value."); // VTol out of range.
}
else
SERIAL_ECHOLNPGM("Wrong mode for vtol parameter.");
}

#endif // HAS_Y_AXIS

#if HAS_X_AXIS
if (flag.update_n) ftMotion.refreshShapingN();
if (flag.update_a) ftMotion.updateShapingA();
#endif
if (flag.reset_ft) ftMotion.reset();
if (flag.report_h) say_shaping();
planner.synchronize();

if (flag.update_n) ftMotion.refreshShapingN();

if (flag.update_a) ftMotion.updateShapingA();

if (flag.reset_ft) {
stepper.ftMotion_syncPosition();
ftMotion.reset();
}

if (flag.report_h) say_shaping();
}

#endif // FT_MOTION
1 change: 1 addition & 0 deletions Marlin/src/gcode/probe/G38.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -105,6 +105,7 @@ inline bool G38_run_probe() {
* G38.5 - Probe away from workpiece, stop on contact break
*/
void GcodeSuite::G38(const int8_t subcode) {

// Get X Y Z E F
get_destination_from_command();

Expand Down
2 changes: 1 addition & 1 deletion Marlin/src/inc/Conditionals_adv.h
Original file line number Diff line number Diff line change
Expand Up @@ -1144,7 +1144,7 @@
#elif HAS_DRIVER(A4988)
#define MINIMUM_STEPPER_POST_DIR_DELAY 200
#elif HAS_TRINAMIC_CONFIG || HAS_TRINAMIC_STANDALONE
#define MINIMUM_STEPPER_POST_DIR_DELAY 60
#define MINIMUM_STEPPER_POST_DIR_DELAY 70
#else
#define MINIMUM_STEPPER_POST_DIR_DELAY 0 // Expect at least 10µS since one Stepper ISR must transpire
#endif
Expand Down
6 changes: 4 additions & 2 deletions Marlin/src/lcd/language/language_en.h
Original file line number Diff line number Diff line change
Expand Up @@ -802,6 +802,8 @@ namespace LanguageNarrow_en {
LSTR MSG_FTM_MODE = _UxGT("Motion Mode:");
LSTR MSG_FTM_ZV = _UxGT("ZV");
LSTR MSG_FTM_ZVD = _UxGT("ZVD");
LSTR MSG_FTM_ZVDD = _UxGT("ZVDD");
LSTR MSG_FTM_ZVDDD = _UxGT("ZVDDD");
LSTR MSG_FTM_EI = _UxGT("EI");
LSTR MSG_FTM_2HEI = _UxGT("2HEI");
LSTR MSG_FTM_3HEI = _UxGT("3HEI");
Expand All @@ -813,8 +815,8 @@ namespace LanguageNarrow_en {
LSTR MSG_FTM_MASS_BASED = _UxGT("Mass-based");
LSTR MSG_FTM_BASE_FREQ_N = _UxGT("@ Base Freq.");
LSTR MSG_FTM_DFREQ_K_N = _UxGT("@ Dyn. Freq.");
LSTR MSG_FTM_ZETA = _UxGT("Damping");
LSTR MSG_FTM_VTOL = _UxGT("Vib. Level");
LSTR MSG_FTM_ZETA_N = _UxGT("@ Damping");
LSTR MSG_FTM_VTOL_N = _UxGT("@ Vib. Level");

LSTR MSG_LEVEL_X_AXIS = _UxGT("Level X Axis");
LSTR MSG_AUTO_CALIBRATE = _UxGT("Auto Calibrate");
Expand Down
Loading

0 comments on commit d9f3d5f

Please sign in to comment.