3DPrinting/MarlinFirmware
1
0
Fork 0
mirror of https://github.com/MarlinFirmware/Marlin.git synced 2024-11-26 13:25:54 +00:00
Code Issues Releases Wiki Activity

🧑‍💻 FT Motion: Individual axis shaping, new buffer management (#26848)

Browse Source 
Co-authored-by: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
narno2202 2024-07-15 20:13:00 +02:00 committed by GitHub
parent 20a704b154
commit f0bc4274f8
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
13 changed files with 500 additions and 650 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Marlin/Configuration_adv.h
View File

@ -1118,11 +1118,14 @@
/**
* Fixed-time-based Motion Control -- EXPERIMENTAL
* Enable/disable and set parameters with G-code M493.
* See ft_types.h for named values used by FTM options.
*/
//#define FT_MOTION
#if ENABLED(FT_MOTION)
#define FTM_DEFAULT_MODE ftMotionMode_DISABLED // Default mode of fixed time control. (Enums in ft_types.h)
#define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (Enums in ft_types.h)
//#define FTM_IS_DEFAULT_MOTION // Use FT Motion as the factory default?
#define FTM_DEFAULT_DYNFREQ_MODE dynFreqMode_DISABLED // Default mode of dynamic frequency calculation. (DISABLED, Z_BASED, MASS_BASED)
#define FTM_DEFAULT_SHAPER_X ftMotionShaper_NONE // Default shaper mode on X axis (NONE, ZV, ZVD, ZVDD, ZVDDD, EI, 2HEI, 3HEI, MZV)
#define FTM_DEFAULT_SHAPER_Y ftMotionShaper_NONE // Default shaper mode on Y axis
#define FTM_SHAPING_DEFAULT_X_FREQ 37.0f // (Hz) Default peak frequency used by input shapers
#define FTM_SHAPING_DEFAULT_Y_FREQ 37.0f // (Hz) Default peak frequency used by input shapers
#define FTM_LINEAR_ADV_DEFAULT_ENA false // Default linear advance enable (true) or disable (false)
@ -1149,18 +1152,13 @@
#define FTM_FS 1000 // (Hz) Frequency for trajectory generation. (Reciprocal of FTM_TS)
#define FTM_TS 0.001f // (s) Time step for trajectory generation. (Reciprocal of FTM_FS)
// These values may be configured to adjust the duration of loop().
#define FTM_STEPS_PER_LOOP 60 // Number of stepper commands to generate each loop()
#define FTM_POINTS_PER_LOOP 100 // Number of trajectory points to generate each loop()
#if DISABLED(COREXY)
#define FTM_STEPPER_FS 20000 // (Hz) Frequency for stepper I/O update
// Use this to adjust the time required to consume the command buffer.
// Try increasing this value if stepper motion is choppy.
#define FTM_STEPPERCMD_BUFF_SIZE 3000 // Size of the stepper command buffers
// (FTM_STEPS_PER_LOOP * FTM_POINTS_PER_LOOP) is a good start
// If you run out of memory, fall back to 3000 and increase progressively
#else
// CoreXY motion needs a larger buffer size. These values are based on our testing.
#define FTM_STEPPER_FS 30000

2
Marlin/src/HAL/ESP32/i2s.cpp
View File

@ -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) {

180
Marlin/src/gcode/feature/ft_motion/M493.cpp
View File

@ -28,30 +28,51 @@
#include "../../../module/ft_motion.h"
#include "../../../module/stepper.h"
void say_shaper_type(const AxisEnum a) {
SERIAL_ECHOPGM(" axis ");
switch (ftMotion.cfg.shaper[a]) {
default: break;
case ftMotionShaper_ZV: SERIAL_ECHOPGM("ZV"); break;
case ftMotionShaper_ZVD: SERIAL_ECHOPGM("ZVD"); break;
case ftMotionShaper_ZVDD: SERIAL_ECHOPGM("ZVDD"); break;
case ftMotionShaper_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break;
case ftMotionShaper_EI: SERIAL_ECHOPGM("EI"); break;
case ftMotionShaper_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break;
case ftMotionShaper_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break;
case ftMotionShaper_MZV: SERIAL_ECHOPGM("MZV"); break;
}
SERIAL_ECHOPGM(" shaping");
}
#if CORE_IS_XY || CORE_IS_XZ
#define AXIS_0_NAME "A"
#else
#define AXIS_0_NAME "X"
#endif
#if CORE_IS_XY || CORE_IS_YZ
#define AXIS_1_NAME "B"
#else
#define AXIS_1_NAME "Y"
#endif
void say_shaping() {
// FT Enabled
SERIAL_ECHO_TERNARY(ftMotion.cfg.mode, "Fixed-Time Motion ", "en", "dis", "abled");
SERIAL_ECHO_TERNARY(ftMotion.cfg.active, "Fixed-Time Motion ", "en", "dis", "abled");
// FT Shaping
#if HAS_X_AXIS
if (ftMotion.cfg.mode > ftMotionMode_ENABLED) {
SERIAL_ECHOPGM(" with ");
switch (ftMotion.cfg.mode) {
default: break;
case ftMotionMode_ZV: SERIAL_ECHOPGM("ZV"); break;
case ftMotionMode_ZVD: SERIAL_ECHOPGM("ZVD"); break;
case ftMotionMode_ZVDD: SERIAL_ECHOPGM("ZVDD"); break;
case ftMotionMode_ZVDDD: SERIAL_ECHOPGM("ZVDDD"); break;
case ftMotionMode_EI: SERIAL_ECHOPGM("EI"); break;
case ftMotionMode_2HEI: SERIAL_ECHOPGM("2 Hump EI"); break;
case ftMotionMode_3HEI: SERIAL_ECHOPGM("3 Hump EI"); break;
case ftMotionMode_MZV: SERIAL_ECHOPGM("MZV"); break;
//case ftMotionMode_DISCTF: SERIAL_ECHOPGM("discrete transfer functions"); break;
//case ftMotionMode_ULENDO_FBS: SERIAL_ECHOPGM("Ulendo FBS."); return;
}
SERIAL_ECHOPGM(" shaping");
if (CMPNSTR_HAS_SHAPER(X_AXIS)) {
SERIAL_ECHOPGM(" with " AXIS_0_NAME);
say_shaper_type(X_AXIS);
}
#endif
#if HAS_Y_AXIS
if (CMPNSTR_HAS_SHAPER(Y_AXIS)) {
SERIAL_ECHOPGM(" and with " AXIS_1_NAME);
say_shaper_type(Y_AXIS);
}
#endif
SERIAL_ECHOLNPGM(".");
const bool z_based = TERN0(HAS_DYNAMIC_FREQ_MM, ftMotion.cfg.dynFreqMode == dynFreqMode_Z_BASED),
@ -59,7 +80,7 @@ void say_shaping() {
dynamic = z_based || g_based;
// FT Dynamic Frequency Mode
if (ftMotion.cfg.modeHasShaper()) {
if (CMPNSTR_HAS_SHAPER(X_AXIS) || CMPNSTR_HAS_SHAPER(Y_AXIS)) {
#if HAS_DYNAMIC_FREQ
SERIAL_ECHOPGM("Dynamic Frequency Mode ");
switch (ftMotion.cfg.dynFreqMode) {
@ -76,7 +97,7 @@ void say_shaping() {
#endif
#if HAS_X_AXIS
SERIAL_ECHO_TERNARY(dynamic, "X/A ", "base dynamic", "static", " compensator frequency: ");
SERIAL_ECHO_TERNARY(dynamic, AXIS_0_NAME " ", "base dynamic", "static", " shaper frequency: ");
SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[X_AXIS], 2), F("Hz"));
#if HAS_DYNAMIC_FREQ
if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK[X_AXIS], 2), F("Hz/"), z_based ? F("mm") : F("g"));
@ -85,7 +106,7 @@ void say_shaping() {
#endif
#if HAS_Y_AXIS
SERIAL_ECHO_TERNARY(dynamic, "Y/B ", "base dynamic", "static", " compensator frequency: ");
SERIAL_ECHO_TERNARY(dynamic, AXIS_1_NAME " ", "base dynamic", "static", " shaper frequency: ");
SERIAL_ECHO(p_float_t(ftMotion.cfg.baseFreq[Y_AXIS], 2), F(" Hz"));
#if HAS_DYNAMIC_FREQ
if (dynamic) SERIAL_ECHO(F(" scaling: "), p_float_t(ftMotion.cfg.dynFreqK[Y_AXIS], 2), F("Hz/"), z_based ? F("mm") : F("g"));
@ -108,7 +129,7 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) {
report_heading_etc(forReplay, F(STR_FT_MOTION));
const ft_config_t &c = ftMotion.cfg;
SERIAL_ECHOPGM(" M493 S", c.mode);
SERIAL_ECHOPGM(" M493 S", c.active);
#if HAS_X_AXIS
SERIAL_ECHOPGM(" A", c.baseFreq[X_AXIS]);
#if HAS_Y_AXIS
@ -133,18 +154,21 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) {
/**
* M493: Set Fixed-time Motion Control parameters
*
* S<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
*
@ -166,40 +190,56 @@ void GcodeSuite::M493_report(const bool forReplay/*=true*/) {
* R 0.00 Set the vibration tolerance for the Y axis
*/
void GcodeSuite::M493() {
struct { bool update_n:1, update_a:1, reset_ft:1, report_h:1; } flag = { false };
struct { bool update:1, reset_ft:1, report_h:1; } flag = { false };
if (!parser.seen_any())
flag.report_h = true;
// Parse 'S' mode parameter.
if (parser.seenval('S')) {
const ftMotionMode_t newmm = (ftMotionMode_t)parser.value_byte();
if (parser.seen('S')) {
const bool active = parser.value_bool();
if (newmm != ftMotion.cfg.mode) {
switch (newmm) {
default: SERIAL_ECHOLNPGM("?Invalid control mode [S] value."); return;
#if HAS_X_AXIS
case ftMotionMode_ZV:
case ftMotionMode_ZVD:
case ftMotionMode_ZVDD:
case ftMotionMode_ZVDDD:
case ftMotionMode_EI:
case ftMotionMode_2HEI:
case ftMotionMode_3HEI:
case ftMotionMode_MZV:
//case ftMotionMode_ULENDO_FBS:
//case ftMotionMode_DISCTF:
flag.update_n = flag.update_a = true;
#endif
case ftMotionMode_DISABLED: flag.reset_ft = true;
case ftMotionMode_ENABLED:
ftMotion.cfg.mode = newmm;
flag.report_h = true;
if (active != ftMotion.cfg.active) {
switch (active) {
case false: flag.reset_ft = true;
case true: flag.report_h = true;
ftMotion.cfg.active = active;
break;
}
}
}
#if HAS_X_AXIS
auto set_shaper = [&](const AxisEnum axis, const char c) {
const ftMotionShaper_t newsh = (ftMotionShaper_t)parser.value_byte();
if (newsh != ftMotion.cfg.shaper[axis]) {
switch (newsh) {
default: SERIAL_ECHOLNPGM("?Invalid [", C(c), "] shaper."); return true;
case ftMotionShaper_NONE:
case ftMotionShaper_ZV:
case ftMotionShaper_ZVD:
case ftMotionShaper_ZVDD:
case ftMotionShaper_ZVDDD:
case ftMotionShaper_EI:
case ftMotionShaper_2HEI:
case ftMotionShaper_3HEI:
case ftMotionShaper_MZV:
ftMotion.cfg.shaper[axis] = newsh;
flag.update = flag.report_h = true;
break;
}
}
return false;
};
if (parser.seenval('X') && set_shaper(X_AXIS, 'X')) return; // Parse 'X' mode parameter
#if HAS_Y_AXIS
if (parser.seenval('Y') && set_shaper(Y_AXIS, 'Y')) return; // Parse 'Y' mode parameter
#endif
#endif // HAS_X_AXIS
#if HAS_EXTRUDERS
// Pressure control (linear advance) parameter.
@ -227,7 +267,7 @@ void GcodeSuite::M493() {
// Dynamic frequency mode parameter.
if (parser.seenval('D')) {
if (ftMotion.cfg.modeHasShaper()) {
if (CMPNSTR_HAS_SHAPER(X_AXIS) || CMPNSTR_HAS_SHAPER(Y_AXIS)) {
const dynFreqMode_t val = dynFreqMode_t(parser.value_byte());
switch (val) {
#if HAS_DYNAMIC_FREQ_MM
@ -261,12 +301,12 @@ void GcodeSuite::M493() {
// Parse frequency parameter (X axis).
if (parser.seenval('A')) {
if (ftMotion.cfg.modeHasShaper()) {
if (CMPNSTR_HAS_SHAPER(X_AXIS)) {
const float val = parser.value_float();
// TODO: Frequency minimum is dependent on the shaper used; the above check isn't always correct.
if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) {
ftMotion.cfg.baseFreq[X_AXIS] = val;
flag.update_n = flag.reset_ft = flag.report_h = true;
flag.update = flag.reset_ft = flag.report_h = true;
}
else // Frequency out of range.
SERIAL_ECHOLNPGM("Invalid [", C('A'), "] frequency value.");
@ -290,10 +330,10 @@ void GcodeSuite::M493() {
// Parse zeta parameter (X axis).
if (parser.seenval('I')) {
const float val = parser.value_float();
if (ftMotion.cfg.modeHasShaper()) {
if (CMPNSTR_HAS_SHAPER(X_AXIS)) {
if (WITHIN(val, 0.01f, 1.0f)) {
ftMotion.cfg.zeta[0] = val;
flag.update_n = flag.update_a = true;
flag.update = true;
}
else
SERIAL_ECHOLNPGM("Invalid X zeta [", C('I'), "] value."); // Zeta out of range.
@ -305,10 +345,10 @@ void GcodeSuite::M493() {
// Parse vtol parameter (X axis).
if (parser.seenval('Q')) {
const float val = parser.value_float();
if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) {
if (CMPNSTR_IS_EISHAPER(X_AXIS)) {
if (WITHIN(val, 0.00f, 1.0f)) {
ftMotion.cfg.vtol[0] = val;
flag.update_a = true;
flag.update = true;
}
else
SERIAL_ECHOLNPGM("Invalid X vtol [", C('Q'), "] value."); // VTol out of range.
@ -323,11 +363,11 @@ void GcodeSuite::M493() {
// Parse frequency parameter (Y axis).
if (parser.seenval('B')) {
if (ftMotion.cfg.modeHasShaper()) {
if (CMPNSTR_HAS_SHAPER(Y_AXIS)) {
const float val = parser.value_float();
if (WITHIN(val, FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2)) {
ftMotion.cfg.baseFreq[Y_AXIS] = val;
flag.update_n = flag.reset_ft = flag.report_h = true;
flag.update = flag.reset_ft = flag.report_h = true;
}
else // Frequency out of range.
SERIAL_ECHOLNPGM("Invalid frequency [", C('B'), "] value.");
@ -351,10 +391,10 @@ void GcodeSuite::M493() {
// Parse zeta parameter (Y axis).
if (parser.seenval('J')) {
const float val = parser.value_float();
if (ftMotion.cfg.modeHasShaper()) {
if (CMPNSTR_HAS_SHAPER(Y_AXIS)) {
if (WITHIN(val, 0.01f, 1.0f)) {
ftMotion.cfg.zeta[1] = val;
flag.update_n = flag.update_a = true;
flag.update = true;
}
else
SERIAL_ECHOLNPGM("Invalid Y zeta [", C('J'), "] value."); // Zeta Out of range
@ -366,10 +406,10 @@ void GcodeSuite::M493() {
// Parse vtol parameter (Y axis).
if (parser.seenval('R')) {
const float val = parser.value_float();
if (ftMotion.cfg.modeHasShaper() && IS_EI_MODE(ftMotion.cfg.mode)) {
if (CMPNSTR_IS_EISHAPER(Y_AXIS)) {
if (WITHIN(val, 0.00f, 1.0f)) {
ftMotion.cfg.vtol[1] = val;
flag.update_a = true;
flag.update = true;
}
else
SERIAL_ECHOLNPGM("Invalid Y vtol [", C('R'), "] value."); // VTol out of range.
@ -382,9 +422,7 @@ void GcodeSuite::M493() {
planner.synchronize();
if (flag.update_n) ftMotion.refreshShapingN();
if (flag.update_a) ftMotion.updateShapingA();
if (flag.update) ftMotion.update_shaping_params();
if (flag.reset_ft) {
stepper.ftMotion_syncPosition();

9
Marlin/src/inc/SanityCheck.h
View File

@ -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

2
Marlin/src/lcd/language/language_en.h
View File

@ -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");

141
Marlin/src/lcd/menu/menu_motion.cpp
View File

@ -325,39 +325,51 @@ void menu_move() {
#include "../../module/ft_motion.h"
#include "../../gcode/gcode.h"
void ftm_menu_setShaping(const ftMotionMode_t s) {
ftMotion.cfg.mode = s;
ftMotion.refreshShapingN();
void ftm_menu_set_cmpn(const AxisEnum axis, const ftMotionShaper_t s) {
ftMotion.cfg.shaper[axis] = s;
ftMotion.update_shaping_params();
ui.go_back();
}
inline void menu_ftm_mode() {
const ftMotionMode_t mode = ftMotion.cfg.mode;
inline void menu_ftm_cmpn_x() {
const ftMotionShaper_t shaper = ftMotion.cfg.shaper[X_AXIS];
START_MENU();
BACK_ITEM(MSG_FIXED_TIME_MOTION);
if (mode != ftMotionMode_DISABLED) ACTION_ITEM(MSG_LCD_OFF, []{ ftm_menu_setShaping(ftMotionMode_DISABLED); });
if (mode != ftMotionMode_ENABLED) ACTION_ITEM(MSG_LCD_ON, []{ ftm_menu_setShaping(ftMotionMode_ENABLED); });
#if HAS_X_AXIS
if (mode != ftMotionMode_ZV) ACTION_ITEM(MSG_FTM_ZV, []{ ftm_menu_setShaping(ftMotionMode_ZV); });
if (mode != ftMotionMode_ZVD) ACTION_ITEM(MSG_FTM_ZVD, []{ ftm_menu_setShaping(ftMotionMode_ZVD); });
if (mode != ftMotionMode_ZVDD) ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_setShaping(ftMotionMode_ZVDD); });
if (mode != ftMotionMode_ZVDDD) ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_setShaping(ftMotionMode_ZVDDD); });
if (mode != ftMotionMode_EI) ACTION_ITEM(MSG_FTM_EI, []{ ftm_menu_setShaping(ftMotionMode_EI); });
if (mode != ftMotionMode_2HEI) ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_setShaping(ftMotionMode_2HEI); });
if (mode != ftMotionMode_3HEI) ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_setShaping(ftMotionMode_3HEI); });
if (mode != ftMotionMode_MZV) ACTION_ITEM(MSG_FTM_MZV, []{ ftm_menu_setShaping(ftMotionMode_MZV); });
//if (mode != ftMotionMode_ULENDO_FBS) ACTION_ITEM(MSG_FTM_ULENDO_FBS, []{ ftm_menu_setShaping(ftMotionMode_ULENDO_FBS); });
//if (mode != ftMotionMode_DISCTF) ACTION_ITEM(MSG_FTM_DISCTF, []{ ftm_menu_setShaping(ftMotionMode_DISCTF); });
#endif
if (shaper != ftMotionShaper_NONE) ACTION_ITEM(MSG_LCD_OFF, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_NONE); });
if (shaper != ftMotionShaper_ZV) ACTION_ITEM(MSG_FTM_ZV, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_ZV); });
if (shaper != ftMotionShaper_ZVD) ACTION_ITEM(MSG_FTM_ZVD, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_ZVD); });
if (shaper != ftMotionShaper_ZVDD) ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_ZVDD); });
if (shaper != ftMotionShaper_ZVDDD) ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_ZVDDD); });
if (shaper != ftMotionShaper_EI) ACTION_ITEM(MSG_FTM_EI, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_EI); });
if (shaper != ftMotionShaper_2HEI) ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_2HEI); });
if (shaper != ftMotionShaper_3HEI) ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_3HEI); });
if (shaper != ftMotionShaper_MZV) ACTION_ITEM(MSG_FTM_MZV, []{ ftm_menu_set_cmpn(X_AXIS, ftMotionShaper_MZV); });
END_MENU();
}
inline void menu_ftm_cmpn_y() {
const ftMotionShaper_t shaper = ftMotion.cfg.shaper[Y_AXIS];
START_MENU();
BACK_ITEM(MSG_FIXED_TIME_MOTION);
if (shaper != ftMotionShaper_NONE) ACTION_ITEM(MSG_LCD_OFF, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_NONE); });
if (shaper != ftMotionShaper_ZV) ACTION_ITEM(MSG_FTM_ZV, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_ZV); });
if (shaper != ftMotionShaper_ZVD) ACTION_ITEM(MSG_FTM_ZVD, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_ZVD); });
if (shaper != ftMotionShaper_ZVDD) ACTION_ITEM(MSG_FTM_ZVDD, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_ZVDD); });
if (shaper != ftMotionShaper_ZVDDD) ACTION_ITEM(MSG_FTM_ZVDDD,[]{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_ZVDDD); });
if (shaper != ftMotionShaper_EI) ACTION_ITEM(MSG_FTM_EI, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_EI); });
if (shaper != ftMotionShaper_2HEI) ACTION_ITEM(MSG_FTM_2HEI, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_2HEI); });
if (shaper != ftMotionShaper_3HEI) ACTION_ITEM(MSG_FTM_3HEI, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_3HEI); });
if (shaper != ftMotionShaper_MZV) ACTION_ITEM(MSG_FTM_MZV, []{ ftm_menu_set_cmpn(Y_AXIS, ftMotionShaper_MZV); });
END_MENU();
}
#if HAS_DYNAMIC_FREQ
inline void menu_ftm_dyn_mode() {
void menu_ftm_dyn_mode() {
const dynFreqMode_t dmode = ftMotion.cfg.dynFreqMode;
START_MENU();
@ -379,22 +391,23 @@ void menu_move() {
void menu_ft_motion() {
ft_config_t &c = ftMotion.cfg;
FSTR_P ftmode;
switch (c.mode) {
default:
case ftMotionMode_DISABLED: ftmode = GET_TEXT_F(MSG_LCD_OFF); break;
case ftMotionMode_ENABLED: ftmode = GET_TEXT_F(MSG_LCD_ON); break;
case ftMotionMode_ZV: ftmode = GET_TEXT_F(MSG_FTM_ZV); break;
case ftMotionMode_ZVD: ftmode = GET_TEXT_F(MSG_FTM_ZVD); break;
case ftMotionMode_ZVDD: ftmode = GET_TEXT_F(MSG_FTM_ZVDD); break;
case ftMotionMode_ZVDDD: ftmode = GET_TEXT_F(MSG_FTM_ZVDDD);break;
case ftMotionMode_EI: ftmode = GET_TEXT_F(MSG_FTM_EI); break;
case ftMotionMode_2HEI: ftmode = GET_TEXT_F(MSG_FTM_2HEI); break;
case ftMotionMode_3HEI: ftmode = GET_TEXT_F(MSG_FTM_3HEI); break;
case ftMotionMode_MZV: ftmode = GET_TEXT_F(MSG_FTM_MZV); break;
//case ftMotionMode_ULENDO_FBS: ftmode = GET_TEXT_F(MSG_FTM_ULENDO_FBS); break;
//case ftMotionMode_DISCTF: ftmode = GET_TEXT_F(MSG_FTM_DISCTF); break;
}
FSTR_P ftshaper[1 + ENABLED(HAS_Y_AXIS)] {};
#if HAS_X_AXIS
for (uint_fast8_t a = X_AXIS; a <= TERN(HAS_Y_AXIS, Y_AXIS, X_AXIS); ++a) {
switch (c.shaper[a]) {
case ftMotionShaper_NONE: ftshaper[a] = GET_TEXT_F(MSG_LCD_OFF); break;
case ftMotionShaper_ZV: ftshaper[a] = GET_TEXT_F(MSG_FTM_ZV); break;
case ftMotionShaper_ZVD: ftshaper[a] = GET_TEXT_F(MSG_FTM_ZVD); break;
case ftMotionShaper_ZVDD: ftshaper[a] = GET_TEXT_F(MSG_FTM_ZVDD); break;
case ftMotionShaper_ZVDDD: ftshaper[a] = GET_TEXT_F(MSG_FTM_ZVDDD);break;
case ftMotionShaper_EI: ftshaper[a] = GET_TEXT_F(MSG_FTM_EI); break;
case ftMotionShaper_2HEI: ftshaper[a] = GET_TEXT_F(MSG_FTM_2HEI); break;
case ftMotionShaper_3HEI: ftshaper[a] = GET_TEXT_F(MSG_FTM_3HEI); break;
case ftMotionShaper_MZV: ftshaper[a] = GET_TEXT_F(MSG_FTM_MZV); break;
}
}
#endif
#if HAS_DYNAMIC_FREQ
FSTR_P dmode;
@ -409,32 +422,35 @@ void menu_move() {
START_MENU();
BACK_ITEM(MSG_MOTION);
SUBMENU(MSG_FTM_MODE, menu_ftm_mode);
MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(ftmode); MENU_ITEM_ADDON_END();
bool show_state = ftMotion.cfg.active;
EDIT_ITEM(bool, MSG_FIXED_TIME_MOTION, &show_state, []{
ftMotion.cfg.active ^= true;
ftMotion.update_shaping_params();
});
if (c.modeHasShaper()) {
if (c.active) {
#if HAS_X_AXIS
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[X_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.refreshShapingN);
SUBMENU_N(X_AXIS, MSG_FTM_CMPN_MODE, menu_ftm_cmpn_x);
MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(ftshaper[X_AXIS]); MENU_ITEM_ADDON_END();
if (CMPNSTR_HAS_SHAPER(X_AXIS)) {
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[X_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params);
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta[0], 0.0f, 1.0f, ftMotion.update_shaping_params);
if (CMPNSTR_IS_EISHAPER(X_AXIS))
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol[0], 0.0f, 1.0f, ftMotion.update_shaping_params);
}
#endif
#if HAS_Y_AXIS
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[Y_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.refreshShapingN);
#endif
SUBMENU_N(Y_AXIS, MSG_FTM_CMPN_MODE, menu_ftm_cmpn_y);
MENU_ITEM_ADDON_START_RJ(5); lcd_put_u8str(ftshaper[Y_AXIS]); MENU_ITEM_ADDON_END();
#if HAS_X_AXIS
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_ZETA_N, &c.zeta[0], 0.0f, 1.0f, ftMotion.refreshShapingN);
if (CMPNSTR_HAS_SHAPER(Y_AXIS)) {
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_BASE_FREQ_N, &c.baseFreq[Y_AXIS], FTM_MIN_SHAPE_FREQ, (FTM_FS) / 2, ftMotion.update_shaping_params);
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta[1], 0.0f, 1.0f, ftMotion.update_shaping_params);
if (CMPNSTR_IS_EISHAPER(Y_AXIS))
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol[1], 0.0f, 1.0f, ftMotion.update_shaping_params);
}
#endif
#if HAS_Y_AXIS
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_ZETA_N, &c.zeta[1], 0.0f, 1.0f, ftMotion.refreshShapingN);
#endif
if (IS_EI_MODE(c.mode)) {
#if HAS_X_AXIS
EDIT_ITEM_FAST_N(float42_52, X_AXIS, MSG_FTM_VTOL_N, &c.vtol[0], 0.0f, 1.0f, ftMotion.refreshShapingN);
#endif
#if HAS_Y_AXIS
EDIT_ITEM_FAST_N(float42_52, Y_AXIS, MSG_FTM_VTOL_N, &c.vtol[1], 0.0f, 1.0f, ftMotion.refreshShapingN);
#endif
}
#if HAS_DYNAMIC_FREQ
SUBMENU(MSG_FTM_DYN_MODE, menu_ftm_dyn_mode);
@ -448,13 +464,12 @@ void menu_move() {
#endif
}
#endif
#if HAS_EXTRUDERS
EDIT_ITEM(bool, MSG_LINEAR_ADVANCE, &c.linearAdvEna);
if (c.linearAdvEna) EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10);
#endif
}
#if HAS_EXTRUDERS
EDIT_ITEM(bool, MSG_LINEAR_ADVANCE, &c.linearAdvEna);
if (c.linearAdvEna) EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10);
#endif
END_MENU();
}

50
Marlin/src/module/endstops.cpp
View File

@ -50,10 +50,6 @@
#include "../feature/joystick.h"
#endif
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#endif
#if HAS_BED_PROBE
#include "probe.h"
#endif
@ -837,19 +833,9 @@ void Endstops::update() {
#endif
// Signal, after validation, if an endstop limit is pressed or not
bool moving_neg;
auto axis_moving_info = [](const AxisEnum axis, const AxisEnum head, bool &neg) -> bool {
#if ENABLED(FT_MOTION)
if (ftMotion.cfg.mode != ftMotionMode_DISABLED)
return (neg = ftMotion.axis_moving_neg(head)) || ftMotion.axis_moving_pos(head);
#endif
neg = !stepper.motor_direction(head);
return stepper.axis_is_moving(axis);
};
#if HAS_X_AXIS
if (axis_moving_info(X_AXIS, X_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // -direction
if (stepper.axis_is_moving(X_AXIS)) {
if (!stepper.motor_direction(X_AXIS_HEAD)) { // -direction
#if HAS_X_MIN_STATE
PROCESS_ENDSTOP_X(MIN);
#if CORE_DIAG(XY, Y, MIN)
@ -881,8 +867,8 @@ void Endstops::update() {
#endif // HAS_X_AXIS
#if HAS_Y_AXIS
if (axis_moving_info(Y_AXIS, Y_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // -direction
if (stepper.axis_is_moving(Y_AXIS)) {
if (!stepper.motor_direction(Y_AXIS_HEAD)) { // -direction
#if HAS_Y_MIN_STATE
PROCESS_ENDSTOP_Y(MIN);
#if CORE_DIAG(XY, X, MIN)
@ -914,8 +900,8 @@ void Endstops::update() {
#endif // HAS_Y_AXIS
#if HAS_Z_AXIS
if (axis_moving_info(Z_AXIS, Z_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // Z -direction. Gantry down, bed up.
if (stepper.axis_is_moving(Z_AXIS)) {
if (!stepper.motor_direction(Z_AXIS_HEAD)) { // Z -direction. Gantry down, bed up.
#if HAS_Z_MIN_STATE
// If the Z_MIN_PIN is being used for the probe there's no
// separate Z_MIN endstop. But a Z endstop could be wired
@ -959,8 +945,8 @@ void Endstops::update() {
#endif // HAS_Z_AXIS
#if HAS_I_AXIS
if (axis_moving_info(I_AXIS, I_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // -direction
if (stepper.axis_is_moving(I_AXIS)) {
if (!stepper.motor_direction(I_AXIS_HEAD)) { // -direction
#if HAS_I_MIN_STATE
PROCESS_ENDSTOP(I, MIN);
#endif
@ -974,8 +960,8 @@ void Endstops::update() {
#endif // HAS_I_AXIS
#if HAS_J_AXIS
if (axis_moving_info(J_AXIS, J_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // -direction
if (stepper.axis_is_moving(J_AXIS)) {
if (!stepper.motor_direction(J_AXIS_HEAD)) { // -direction
#if HAS_J_MIN_STATE
PROCESS_ENDSTOP(J, MIN);
#endif
@ -989,8 +975,8 @@ void Endstops::update() {
#endif // HAS_J_AXIS
#if HAS_K_AXIS
if (axis_moving_info(K_AXIS, K_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // -direction
if (stepper.axis_is_moving(K_AXIS)) {
if (!stepper.motor_direction(K_AXIS_HEAD)) { // -direction
#if HAS_K_MIN_STATE
PROCESS_ENDSTOP(K, MIN);
#endif
@ -1004,8 +990,8 @@ void Endstops::update() {
#endif // HAS_K_AXIS
#if HAS_U_AXIS
if (axis_moving_info(U_AXIS, U_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // -direction
if (stepper.axis_is_moving(U_AXIS)) {
if (!stepper.motor_direction(U_AXIS_HEAD)) { // -direction
#if HAS_U_MIN_STATE
PROCESS_ENDSTOP(U, MIN);
#endif
@ -1019,8 +1005,8 @@ void Endstops::update() {
#endif // HAS_U_AXIS
#if HAS_V_AXIS
if (axis_moving_info(V_AXIS, V_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // -direction
if (stepper.axis_is_moving(V_AXIS)) {
if (!stepper.motor_direction(V_AXIS_HEAD)) { // -direction
#if HAS_V_MIN_STATE
PROCESS_ENDSTOP(V, MIN);
#endif
@ -1034,8 +1020,8 @@ void Endstops::update() {
#endif // HAS_V_AXIS
#if HAS_W_AXIS
if (axis_moving_info(W_AXIS, W_AXIS_HEAD, moving_neg)) {
if (moving_neg) { // -direction
if (stepper.axis_is_moving(W_AXIS)) {
if (!stepper.motor_direction(W_AXIS_HEAD)) { // -direction
#if HAS_W_MIN_STATE
PROCESS_ENDSTOP(W, MIN);
#endif

494
Marlin/src/module/ft_motion.cpp
View File

@ -30,23 +30,6 @@
FTMotion ftMotion;
#if !HAS_X_AXIS
static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZV, "ftMotionMode_ZV requires at least one linear axis.");
static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVD, "ftMotionMode_ZVD requires at least one linear axis.");
static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVDD, "ftMotionMode_ZVD requires at least one linear axis.");
static_assert(FTM_DEFAULT_MODE == ftMotionMode_ZVDDD, "ftMotionMode_ZVD requires at least one linear axis.");
static_assert(FTM_DEFAULT_MODE == ftMotionMode_EI, "ftMotionMode_EI requires at least one linear axis.");
static_assert(FTM_DEFAULT_MODE == ftMotionMode_2HEI, "ftMotionMode_2HEI requires at least one linear axis.");
static_assert(FTM_DEFAULT_MODE == ftMotionMode_3HEI, "ftMotionMode_3HEI requires at least one linear axis.");
static_assert(FTM_DEFAULT_MODE == ftMotionMode_MZV, "ftMotionMode_MZV requires at least one linear axis.");
#endif
#if !HAS_DYNAMIC_FREQ_MM
static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_Z_BASED, "dynFreqMode_Z_BASED requires a Z axis.");
#endif
#if !HAS_DYNAMIC_FREQ_G
static_assert(FTM_DEFAULT_DYNFREQ_MODE != dynFreqMode_MASS_BASED, "dynFreqMode_MASS_BASED requires an X axis and an extruder.");
#endif
//-----------------------------------------------------------------
// Variables.
//-----------------------------------------------------------------
@ -60,8 +43,6 @@ int32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper comman
FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items and is in use.
millis_t FTMotion::axis_pos_move_end_ti[NUM_AXIS_ENUMS] = {0},
FTMotion::axis_neg_move_end_ti[NUM_AXIS_ENUMS] = {0};
// Private variables.
@ -70,17 +51,15 @@ millis_t FTMotion::axis_pos_move_end_ti[NUM_AXIS_ENUMS] = {0},
xyze_trajectory_t FTMotion::traj; // = {0.0f} Storage for fixed-time-based trajectory.
xyze_trajectoryMod_t FTMotion::trajMod; // = {0.0f} Storage for fixed time trajectory window.
bool FTMotion::blockProcRdy = false, // Indicates a block is ready to be processed.
FTMotion::blockProcRdy_z1 = false, // Storage for the previous indicator.
FTMotion::blockProcDn = false; // Indicates current block is done being processed.
bool FTMotion::batchRdy = false; // Indicates a batch of the fixed time trajectory
// has been generated, is now available in the upper -
// half of traj.x[], y, z ... e vectors, and is ready to be
// post processed, if applicable, then interpolated.
bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done being post processed,
// if applicable, and is ready to be converted to step commands.
bool FTMotion::runoutEna = false; // True if runout of the block hasn't been done and is allowed.
bool FTMotion::blockDataIsRunout = false; // Indicates the last loaded block variables are for a runout.
bool FTMotion::blockProcRdy = false; // Set when new block data is loaded from stepper module into FTM, ...
// ... and reset when block is completely converted to FTM trajectory.
bool FTMotion::batchRdy = false; // Indicates a batch of the fixed time trajectory...
// ... has been generated, is now available in the upper -
// batch of traj.x[], y, z ... e vectors, and is ready to be
// post processed, if applicable, then interpolated. Reset when the
// data has been shifted out.
bool FTMotion::batchRdyForInterp = false; // Indicates the batch is done being post processed...
// ... if applicable, and is ready to be converted to step commands.
// Trapezoid data variables.
xyze_pos_t FTMotion::startPosn, // (mm) Start position of block
@ -101,22 +80,20 @@ uint32_t FTMotion::max_intervals; // Total number of data points t
// Make vector variables.
uint32_t FTMotion::makeVector_idx = 0, // Index of fixed time trajectory generation of the overall block.
FTMotion::makeVector_idx_z1 = 0, // Storage for the previously calculated index above.
FTMotion::makeVector_batchIdx = 0; // Index of fixed time trajectory generation within the batch.
// Interpolation variables.
xyze_long_t FTMotion::steps = { 0 }; // Step count accumulator.
uint32_t FTMotion::interpIdx = 0, // Index of current data point being interpolated.
FTMotion::interpIdx_z1 = 0; // Storage for the previously calculated index above.
uint32_t FTMotion::interpIdx = 0; // Index of current data point being interpolated.
// Shaping variables.
#if HAS_X_AXIS
FTMotion::shaping_t FTMotion::shaping = {
0, 0,
x:{ false, { 0.0f }, { 0.0f }, { 0 } }, // d_zi, Ai, Ni
0,
x:{ false, { 0.0f }, { 0.0f }, { 0 }, { 0 } }, // ena, d_zi, Ai, Ni, max_i
#if HAS_Y_AXIS
y:{ false, { 0.0f }, { 0.0f }, { 0 } } // d_zi, Ai, Ni
y:{ false, { 0.0f }, { 0.0f }, { 0 }, { 0 } } // ena, d_zi, Ai, Ni, max_i
#endif
};
#endif
@ -127,7 +104,7 @@ uint32_t FTMotion::interpIdx = 0, // Index of current data point b
float FTMotion::e_advanced_z1 = 0.0f; // (ms) Unit delay of advanced extruder position.
#endif
constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
constexpr uint32_t BATCH_SIDX_IN_WINDOW = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE); // Batch start index in window.
//-----------------------------------------------------------------
// Function definitions.
@ -137,41 +114,10 @@ constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
static bool markBlockStart = false;
// Sets controller states to begin processing a block.
// Called by Stepper::ftMotion_blockQueueUpdate, invoked from the main loop.
void FTMotion::startBlockProc() {
blockProcRdy = true;
blockProcDn = false;
runoutEna = true;
}
// Move any free data points to the stepper buffer even if a full batch isn't ready.
void FTMotion::runoutBlock() {
if (!runoutEna) return;
startPosn = endPosn_prevBlock;
ratio.reset();
max_intervals = cfg.modeHasShaper() ? shaper_intervals : 0;
if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BATCH_SIZE, min_max_intervals - (FTM_BATCH_SIZE)))
max_intervals = min_max_intervals;
max_intervals += (
#if ENABLED(FTM_UNIFIED_BWS)
FTM_WINDOW_SIZE - makeVector_batchIdx
#else
FTM_WINDOW_SIZE - ((last_batchIdx < (FTM_BATCH_SIZE)) ? 0 : makeVector_batchIdx)
#endif
);
blockProcRdy = blockDataIsRunout = true;
runoutEna = blockProcDn = false;
}
// Controller main, to be invoked from non-isr task.
void FTMotion::loop() {
if (!cfg.mode) return;
if (!cfg.active) return;
/**
* Handle block abort with the following sequence:
@ -182,24 +128,49 @@ void FTMotion::loop() {
*/
if (stepper.abort_current_block) {
if (sts_stepperBusy) return; // Wait until motion buffers are emptied
discard_planner_block_protected();
reset();
blockProcDn = true; // Set queueing to look for next block.
stepper.abort_current_block = false; // Abort finished.
}
// Planner processing and block conversion.
if (!blockProcRdy) stepper.ftMotion_blockQueueUpdate();
while (!blockProcRdy && (stepper.current_block = planner.get_current_block())) {
if (stepper.current_block->is_sync()) { // Sync block?
if (stepper.current_block->is_sync_pos()) // Position sync? Set the position.
stepper._set_position(stepper.current_block->position);
discard_planner_block_protected();
continue;
}
loadBlockData(stepper.current_block);
markBlockStart = true;
blockProcRdy = true;
// Some kinematics track axis motion in HX, HY, HZ
#if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX)
stepper.last_direction_bits.hx = stepper.current_block->direction_bits.hx;
#endif
#if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX)
stepper.last_direction_bits.hy = stepper.current_block->direction_bits.hy;
#endif
#if ANY(CORE_IS_XZ, CORE_IS_YZ)
stepper.last_direction_bits.hz = stepper.current_block->direction_bits.hz;
#endif
}
if (blockProcRdy) {
if (!blockProcRdy_z1) { // One-shot.
if (!blockDataIsRunout) {
loadBlockData(stepper.current_block);
markBlockStart = true;
if (!batchRdy) makeVector(); // Caution: Do not consolidate checks on blockProcRdy/batchRdy, as they are written by makeVector().
// When makeVector is finished: either blockProcRdy has been set false (because the block is
// done being processed) or batchRdy is set true, or both.
// Check if the block has been completely converted:
if (!blockProcRdy) {
discard_planner_block_protected();
// Check if the block needs to be runout:
if (!batchRdy && !planner.movesplanned()){
runoutBlock();
makeVector(); // Do an additional makeVector call to guarantee batchRdy set this loop.
}
else blockDataIsRunout = false;
}
while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP)))
makeVector();
}
// FBS / post processing.
@ -215,7 +186,7 @@ void FTMotion::loop() {
LOGICAL_AXIS_MAP_LC(TCOPY);
// Shift the time series back in the window
#define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], last_batchIdx * sizeof(traj.A[0]));
#define TSHIFT(A) memcpy(traj.A, &traj.A[FTM_BATCH_SIZE], BATCH_SIDX_IN_WINDOW * sizeof(traj.A[0]));
LOGICAL_AXIS_MAP_LC(TSHIFT);
#endif
@ -227,9 +198,7 @@ void FTMotion::loop() {
// Interpolation.
while (batchRdyForInterp
&& (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))
&& (interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP))
) {
&& (stepperCmdBuffItems() < (FTM_STEPPERCMD_BUFF_SIZE) - (FTM_STEPS_PER_UNIT_TIME))) {
convertToSteps(interpIdx);
if (++interpIdx == FTM_BATCH_SIZE) {
batchRdyForInterp = false;
@ -238,196 +207,139 @@ void FTMotion::loop() {
}
// Report busy status to planner.
busy = (sts_stepperBusy || ((!blockProcDn && blockProcRdy) || batchRdy || batchRdyForInterp || runoutEna));
busy = (sts_stepperBusy || blockProcRdy || batchRdy || batchRdyForInterp);
blockProcRdy_z1 = blockProcRdy;
makeVector_idx_z1 = makeVector_idx;
interpIdx_z1 = interpIdx;
}
#if HAS_X_AXIS
// Refresh the gains used by shaping functions.
// To be called on init or mode or zeta change.
void FTMotion::AxisShaping::set_axis_shaping_A(const ftMotionShaper_t shaper, const_float_t zeta, const_float_t vtol) {
void FTMotion::Shaping::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) {
const float K = exp(-zeta * M_PI / sqrt(1.f - sq(zeta))),
K2 = sq(K),
K3 = K2 * K,
K4 = K3 * K;
const float Kx = exp(-zeta[0] * M_PI / sqrt(1.0f - sq(zeta[0]))),
Ky = exp(-zeta[1] * M_PI / sqrt(1.0f - sq(zeta[1]))),
Kx2 = sq(Kx),
Ky2 = sq(Ky);
switch (shaper) {
switch (cfg.mode) {
case ftMotionMode_ZV:
case ftMotionShaper_ZV:
max_i = 1U;
x.Ai[0] = 1.0f / (1.0f + Kx);
x.Ai[1] = x.Ai[0] * Kx;
y.Ai[0] = 1.0f / (1.0f + Ky);
y.Ai[1] = y.Ai[0] * Ky;
Ai[0] = 1.0f / (1.0f + K);
Ai[1] = Ai[0] * K;
break;
case ftMotionMode_ZVD:
case ftMotionShaper_ZVD:
max_i = 2U;
x.Ai[0] = 1.0f / (1.0f + 2.0f * Kx + Kx2);
x.Ai[1] = x.Ai[0] * 2.0f * Kx;
x.Ai[2] = x.Ai[0] * Kx2;
y.Ai[0] = 1.0f / (1.0f + 2.0f * Ky + Ky2);
y.Ai[1] = y.Ai[0] * 2.0f * Ky;
y.Ai[2] = y.Ai[0] * Ky2;
Ai[0] = 1.0f / (1.0f + 2.0f * K + K2);
Ai[1] = Ai[0] * 2.0f * K;
Ai[2] = Ai[0] * K2;
break;
case ftMotionMode_ZVDD:
case ftMotionShaper_ZVDD:
max_i = 3U;
x.Ai[0] = 1.0f / (1.0f + 3.0f * Kx + 3.0f * Kx2 + cu(Kx));
x.Ai[1] = x.Ai[0] * 3.0f * Kx;
x.Ai[2] = x.Ai[0] * 3.0f * Kx2;
x.Ai[3] = x.Ai[0] * cu(Kx);
y.Ai[0] = 1.0f / (1.0f + 3.0f * Ky + 3.0f * Ky2 + cu(Ky));
y.Ai[1] = y.Ai[0] * 3.0f * Ky;
y.Ai[2] = y.Ai[0] * 3.0f * Ky2;
y.Ai[3] = y.Ai[0] * cu(Ky);
Ai[0] = 1.0f / (1.0f + 3.0f * K + 3.0f * K2 + K3);
Ai[1] = Ai[0] * 3.0f * K;
Ai[2] = Ai[0] * 3.0f * K2;
Ai[3] = Ai[0] * K3;
break;
case ftMotionMode_ZVDDD:
case ftMotionShaper_ZVDDD:
max_i = 4U;
x.Ai[0] = 1.0f / (1.0f + 4.0f * Kx + 6.0f * Kx2 + 4.0f * cu(Kx) + sq(Kx2));
x.Ai[1] = x.Ai[0] * 4.0f * Kx;
x.Ai[2] = x.Ai[0] * 6.0f * Kx2;
x.Ai[3] = x.Ai[0] * 4.0f * cu(Kx);
x.Ai[4] = x.Ai[0] * sq(Kx2);
y.Ai[0] = 1.0f / (1.0f + 4.0f * Ky + 6.0f * Ky2 + 4.0f * cu(Ky) + sq(Ky2));
y.Ai[1] = y.Ai[0] * 4.0f * Ky;
y.Ai[2] = y.Ai[0] * 6.0f * Ky2;
y.Ai[3] = y.Ai[0] * 4.0f * cu(Ky);
y.Ai[4] = y.Ai[0] * sq(Ky2);
Ai[0] = 1.0f / (1.0f + 4.0f * K + 6.0f * K2 + 4.0f * K3 + K4);
Ai[1] = Ai[0] * 4.0f * K;
Ai[2] = Ai[0] * 6.0f * K2;
Ai[3] = Ai[0] * 4.0f * K3;
Ai[4] = Ai[0] * K4;
break;
case ftMotionMode_EI: {
case ftMotionShaper_EI: {
max_i = 2U;
x.Ai[0] = 0.25f * (1.0f + vtol[0]);
x.Ai[1] = 0.50f * (1.0f - vtol[0]) * Kx;
x.Ai[2] = x.Ai[0] * Kx2;
Ai[0] = 0.25f * (1.0f + vtol);
Ai[1] = 0.50f * (1.0f - vtol) * K;
Ai[2] = Ai[0] * K2;
y.Ai[0] = 0.25f * (1.0f + vtol[1]);
y.Ai[1] = 0.50f * (1.0f - vtol[1]) * Ky;
y.Ai[2] = y.Ai[0] * Ky2;
const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2]);
const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2]);
const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2]);
for (uint32_t i = 0U; i < 3U; i++) {
x.Ai[i] *= X_adj;
y.Ai[i] *= Y_adj;
Ai[i] *= adj;
}
}
break;
case ftMotionMode_2HEI: {
case ftMotionShaper_2HEI: {
max_i = 3U;
const float vtolx2 = sq(vtol[0]);
const float vtolx2 = sq(vtol);
const float X = pow(vtolx2 * (sqrt(1.0f - vtolx2) + 1.0f), 1.0f / 3.0f);
x.Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X);
x.Ai[1] = (0.5f - x.Ai[0]) * Kx;
x.Ai[2] = x.Ai[1] * Kx;
x.Ai[3] = x.Ai[0] * cu(Kx);
Ai[0] = (3.0f * sq(X) + 2.0f * X + 3.0f * vtolx2) / (16.0f * X);
Ai[1] = (0.5f - Ai[0]) * K;
Ai[2] = Ai[1] * K;
Ai[3] = Ai[0] * K3;
const float vtoly2 = sq(vtol[1]);
const float Y = pow(vtoly2 * (sqrt(1.0f - vtoly2) + 1.0f), 1.0f / 3.0f);
y.Ai[0] = (3.0f * sq(Y) + 2.0f * Y + 3.0f * vtoly2) / (16.0f * Y);
y.Ai[1] = (0.5f - y.Ai[0]) * Ky;
y.Ai[2] = y.Ai[1] * Ky;
y.Ai[3] = y.Ai[0] * cu(Ky);
const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3]);
const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2] + y.Ai[3]);
const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3]);
for (uint32_t i = 0U; i < 4U; i++) {
x.Ai[i] *= X_adj;
y.Ai[i] *= Y_adj;
Ai[i] *= adj;
}
}
break;
case ftMotionMode_3HEI: {
case ftMotionShaper_3HEI: {
max_i = 4U;
x.Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol[0] + 2.0f * sqrt( 2.0f * ( vtol[0] + 1.0f ) * vtol[0] ) );
x.Ai[1] = 0.25f * ( 1.0f - vtol[0] ) * Kx;
x.Ai[2] = ( 0.5f * ( 1.0f + vtol[0] ) - 2.0f * x.Ai[0] ) * Kx2;
x.Ai[3] = x.Ai[1] * Kx2;
x.Ai[4] = x.Ai[0] * sq(Kx2);
Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) );
Ai[1] = 0.25f * ( 1.0f - vtol ) * K;
Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * Ai[0] ) * K2;
Ai[3] = Ai[1] * K2;
Ai[4] = Ai[0] * K4;
y.Ai[0] = 0.0625f * (1.0f + 3.0f * vtol[1] + 2.0f * sqrt(2.0f * (vtol[1] + 1.0f) * vtol[1]));
y.Ai[1] = 0.25f * (1.0f - vtol[1]) * Ky;
y.Ai[2] = (0.5f * (1.0f + vtol[1]) - 2.0f * y.Ai[0]) * Ky2;
y.Ai[3] = y.Ai[1] * Ky2;
y.Ai[4] = y.Ai[0] * sq(Ky2);
const float X_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3] + x.Ai[4]);
const float Y_adj = 1.0f / (y.Ai[0] + y.Ai[1] + y.Ai[2] + y.Ai[3] + y.Ai[4]);
const float adj = 1.0f / (Ai[0] + Ai[1] + Ai[2] + Ai[3] + Ai[4]);
for (uint32_t i = 0U; i < 5U; i++) {
x.Ai[i] *= X_adj;
y.Ai[i] *= Y_adj;
Ai[i] *= adj;
}
}
break;
case ftMotionMode_MZV: {
case ftMotionShaper_MZV: {
max_i = 2U;
const float Bx = 1.4142135623730950488016887242097f * Kx;
x.Ai[0] = 1.0f / (1.0f + Bx + Kx2);
x.Ai[1] = x.Ai[0] * Bx;
x.Ai[2] = x.Ai[0] * Kx2;
const float By = 1.4142135623730950488016887242097f * Ky;
y.Ai[0] = 1.0f / (1.0f + By + Ky2);
y.Ai[1] = y.Ai[0] * By;
y.Ai[2] = y.Ai[0] * Ky2;
const float Bx = 1.4142135623730950488016887242097f * K;
Ai[0] = 1.0f / (1.0f + Bx + K2);
Ai[1] = Ai[0] * Bx;
Ai[2] = Ai[0] * K2;
}
break;
default:
ZERO(x.Ai);
ZERO(y.Ai);
ZERO(Ai);
max_i = 0;
}
}
void FTMotion::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) {
shaping.updateShapingA(zeta, vtol);
}
// Refresh the indices used by shaping functions.
// To be called when frequencies change.
void FTMotion::AxisShaping::updateShapingN(const_float_t f, const_float_t df) {
// Protections omitted for DBZ and for index exceeding array length.
switch (cfg.mode) {
case ftMotionMode_ZV:
void FTMotion::AxisShaping::set_axis_shaping_N(const ftMotionShaper_t shaper, const_float_t f, const_float_t zeta) {
// Note that protections are omitted for DBZ and for index exceeding array length.
const float df = sqrt ( 1.f - sq(zeta) );
switch (shaper) {
case ftMotionShaper_ZV:
Ni[1] = round((0.5f / f / df) * (FTM_FS));
break;
case ftMotionMode_ZVD:
case ftMotionMode_EI:
case ftMotionShaper_ZVD:
case ftMotionShaper_EI:
Ni[1] = round((0.5f / f / df) * (FTM_FS));
Ni[2] = Ni[1] + Ni[1];
break;
case ftMotionMode_ZVDD:
case ftMotionMode_2HEI:
case ftMotionShaper_ZVDD:
case ftMotionShaper_2HEI:
Ni[1] = round((0.5f / f / df) * (FTM_FS));
Ni[2] = Ni[1] + Ni[1];
Ni[3] = Ni[2] + Ni[1];
break;
case ftMotionMode_ZVDDD:
case ftMotionMode_3HEI:
case ftMotionShaper_ZVDDD:
case ftMotionShaper_3HEI:
Ni[1] = round((0.5f / f / df) * (FTM_FS));
Ni[2] = Ni[1] + Ni[1];
Ni[3] = Ni[2] + Ni[1];
Ni[4] = Ni[3] + Ni[1];
break;
case ftMotionMode_MZV:
case ftMotionShaper_MZV:
Ni[1] = round((0.375f / f / df) * (FTM_FS));
Ni[2] = Ni[1] + Ni[1];
break;
@ -435,13 +347,20 @@ void FTMotion::loop() {
}
}
void FTMotion::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), float zeta[]/*=cfg.zeta*/) {
const float xdf = sqrt(1.0f - sq(zeta[0]));
shaping.x.updateShapingN(xf, xdf);
void FTMotion::update_shaping_params() {
#if HAS_X_AXIS
shaping.x.ena = CMPNSTR_HAS_SHAPER(X_AXIS);
if (shaping.x.ena) {
shaping.x.set_axis_shaping_A(cfg.shaper[X_AXIS], cfg.zeta[X_AXIS], cfg.vtol[X_AXIS]);
shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], cfg.baseFreq[X_AXIS], cfg.zeta[X_AXIS]);
}
#endif
#if HAS_Y_AXIS
const float ydf = sqrt(1.0f - sq(zeta[1]));
shaping.y.updateShapingN(yf, ydf);
shaping.y.ena = CMPNSTR_HAS_SHAPER(Y_AXIS);
if (shaping.y.ena) {
shaping.y.set_axis_shaping_A(cfg.shaper[Y_AXIS], cfg.zeta[Y_AXIS], cfg.vtol[Y_AXIS]);
shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], cfg.baseFreq[Y_AXIS], cfg.zeta[Y_AXIS]);
}
#endif
}
@ -454,17 +373,17 @@ void FTMotion::reset() {
traj.reset();
blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
batchRdy = batchRdyForInterp = false;
runoutEna = false;
blockProcRdy = batchRdy = batchRdyForInterp = false;
endPosn_prevBlock.reset();
makeVector_idx = makeVector_idx_z1 = 0;
makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MAX(last_batchIdx, FTM_BATCH_SIZE));
makeVector_idx = 0;
makeVector_batchIdx = TERN(FTM_UNIFIED_BWS, 0, _MIN(BATCH_SIDX_IN_WINDOW, FTM_BATCH_SIZE));
steps.reset();
interpIdx = interpIdx_z1 = 0;
interpIdx = 0;
stepper.axis_did_move.reset();
#if HAS_X_AXIS
ZERO(shaping.x.d_zi);
@ -473,13 +392,48 @@ void FTMotion::reset() {
#endif
TERN_(HAS_EXTRUDERS, e_raw_z1 = e_advanced_z1 = 0.0f);
ZERO(axis_pos_move_end_ti);
ZERO(axis_neg_move_end_ti);
}
// Private functions.
void FTMotion::discard_planner_block_protected() {
if (stepper.current_block) { // Safeguard in case current_block must not be null (it will
// be null when the "block" is a runout or generated) in order
// to use planner.release_current_block().
stepper.current_block = nullptr;
planner.release_current_block(); // FTM uses release_current_block() instead of discard_current_block(),
// as in block_phase_isr(). This change is to avoid invoking axis_did_move.reset().
// current_block = nullptr is added to replicate discard without axis_did_move reset.
// Note invoking axis_did_move.reset() causes no issue since FTM's stepper refreshes
// its values every ISR.
}
}
// Sets up a pseudo block to allow motion to settle buffers to empty. This is
// called when the planner has only one block left. The buffers will be filled
// with the last commanded position by setting the startPosn block variable to
// the last position of the previous block and all ratios to zero such that no
// axes' positions are incremented.
void FTMotion::runoutBlock() {
startPosn = endPosn_prevBlock;
ratio.reset();
int32_t n_to_fill_batch = FTM_WINDOW_SIZE - makeVector_batchIdx;
// This line is to be modified for FBS use; do not optimize out.
int32_t n_to_settle_cmpnstr = (TERN_(HAS_X_AXIS, shaping.x.ena) || TERN_(HAS_Y_AXIS, shaping.y.ena )) ? FTM_ZMAX : 0;
int32_t n_to_fill_batch_after_settling = (n_to_settle_cmpnstr > n_to_fill_batch) ?
FTM_BATCH_SIZE - ((n_to_settle_cmpnstr - n_to_fill_batch) % FTM_BATCH_SIZE) : n_to_fill_batch - n_to_settle_cmpnstr;
int32_t n_to_settle_and_fill_batch = n_to_settle_cmpnstr + n_to_fill_batch_after_settling;
max_intervals = PROP_BATCHES * FTM_BATCH_SIZE + n_to_settle_and_fill_batch;
blockProcRdy = true;
}
// Auxiliary function to get number of step commands in the buffer.
int32_t FTMotion::stepperCmdBuffItems() {
const int32_t udiff = stepperCmdBuff_produceIdx - stepperCmdBuff_consumeIdx;
@ -488,10 +442,7 @@ int32_t FTMotion::stepperCmdBuffItems() {
// Initializes storage variables before startup.
void FTMotion::init() {
#if HAS_X_AXIS
refreshShapingN();
updateShapingA();
#endif
update_shaping_params();
reset(); // Precautionary.
}
@ -598,42 +549,6 @@ void FTMotion::loadBlockData(block_t * const current_block) {
endPosn_prevBlock += moveDist;
millis_t move_end_ti = millis() + SEC_TO_MS(FTM_TS*(float)(max_intervals + num_samples_cmpnstr_settle() + (PROP_BATCHES+1)*FTM_BATCH_SIZE) + ((float)FTM_STEPPERCMD_BUFF_SIZE/(float)FTM_STEPPER_FS));
#if CORE_IS_XY
if (moveDist.x > 0.f) axis_pos_move_end_ti[A_AXIS] = move_end_ti;
if (moveDist.y > 0.f) axis_pos_move_end_ti[B_AXIS] = move_end_ti;
if (moveDist.x + moveDist.y > 0.f) axis_pos_move_end_ti[X_HEAD] = move_end_ti;
if (moveDist.x - moveDist.y > 0.f) axis_pos_move_end_ti[Y_HEAD] = move_end_ti;
if (moveDist.x < 0.f) axis_neg_move_end_ti[A_AXIS] = move_end_ti;
if (moveDist.y < 0.f) axis_neg_move_end_ti[B_AXIS] = move_end_ti;
if (moveDist.x + moveDist.y < 0.f) axis_neg_move_end_ti[X_HEAD] = move_end_ti;
if (moveDist.x - moveDist.y < 0.f) axis_neg_move_end_ti[Y_HEAD] = move_end_ti;
#else
if (moveDist.x > 0.f) axis_pos_move_end_ti[X_AXIS] = move_end_ti;
if (moveDist.y > 0.f) axis_pos_move_end_ti[Y_AXIS] = move_end_ti;
if (moveDist.x < 0.f) axis_neg_move_end_ti[X_AXIS] = move_end_ti;
if (moveDist.y < 0.f) axis_neg_move_end_ti[Y_AXIS] = move_end_ti;
#endif
if (moveDist.z > 0.f) axis_pos_move_end_ti[Z_AXIS] = move_end_ti;
if (moveDist.z < 0.f) axis_neg_move_end_ti[Z_AXIS] = move_end_ti;
// if (moveDist.i > 0.f) axis_pos_move_end_ti[I_AXIS] = move_end_ti;
// if (moveDist.i < 0.f) axis_neg_move_end_ti[I_AXIS] = move_end_ti;
// if (moveDist.j > 0.f) axis_pos_move_end_ti[J_AXIS] = move_end_ti;
// if (moveDist.j < 0.f) axis_neg_move_end_ti[J_AXIS] = move_end_ti;
// if (moveDist.k > 0.f) axis_pos_move_end_ti[K_AXIS] = move_end_ti;
// if (moveDist.k < 0.f) axis_neg_move_end_ti[K_AXIS] = move_end_ti;
// if (moveDist.u > 0.f) axis_pos_move_end_ti[U_AXIS] = move_end_ti;
// if (moveDist.u < 0.f) axis_neg_move_end_ti[U_AXIS] = move_end_ti;
// .
// .
// .
// If the endstop is already pressed, endstop interrupts won't invoke
// endstop_triggered and the move will grind. So check here for a
// triggered endstop, which shortly marks the block for discard.
endstops.update();
}
// Generate data points of the trajectory.
@ -679,9 +594,14 @@ void FTMotion::makeVector() {
#if HAS_DYNAMIC_FREQ_MM
case dynFreqMode_Z_BASED:
if (traj.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed.
const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx]
OPTARG(HAS_Y_AXIS, yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx]);
updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ) OPTARG(HAS_Y_AXIS, _MAX(yf, FTM_MIN_SHAPE_FREQ)));
#if HAS_X_AXIS
const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx];
shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], _MAX(xf, FTM_MIN_SHAPE_FREQ), cfg.zeta[X_AXIS]);
#endif
#if HAS_Y_AXIS
const float yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx];
shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], _MAX(yf, FTM_MIN_SHAPE_FREQ), cfg.zeta[Y_AXIS]);
#endif
}
break;
#endif
@ -690,43 +610,49 @@ void FTMotion::makeVector() {
case dynFreqMode_MASS_BASED:
// Update constantly. The optimization done for Z value makes
// less sense for E, as E is expected to constantly change.
updateShapingN( cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx]
OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx]) );
#if HAS_X_AXIS
shaping.x.set_axis_shaping_N(cfg.shaper[X_AXIS], cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.e[makeVector_batchIdx], cfg.zeta[X_AXIS]);
#endif
#if HAS_Y_AXIS
shaping.y.set_axis_shaping_N(cfg.shaper[Y_AXIS], cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.e[makeVector_batchIdx], cfg.zeta[Y_AXIS]);
#endif
break;
#endif
default: break;
}
// Apply shaping if in mode.
// Apply shaping if active on each axis
#if HAS_X_AXIS
if (cfg.modeHasShaper()) {
shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx];
traj.x[makeVector_batchIdx] *= shaping.x.Ai[0];
#if HAS_Y_AXIS
shaping.y.d_zi[shaping.zi_idx] = traj.y[makeVector_batchIdx];
traj.y[makeVector_batchIdx] *= shaping.y.Ai[0];
#endif
for (uint32_t i = 1U; i <= shaping.max_i; i++) {
const uint32_t udiffx = shaping.zi_idx - shaping.x.Ni[i];
traj.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
#if HAS_Y_AXIS
const uint32_t udiffy = shaping.zi_idx - shaping.y.Ni[i];
traj.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
#endif
if (shaping.x.ena) {
shaping.x.d_zi[shaping.zi_idx] = traj.x[makeVector_batchIdx];
traj.x[makeVector_batchIdx] *= shaping.x.Ai[0];
for (uint32_t i = 1U; i <= shaping.x.max_i; i++) {
const uint32_t udiffx = shaping.zi_idx - shaping.x.Ni[i];
traj.x[makeVector_batchIdx] += shaping.x.Ai[i] * shaping.x.d_zi[shaping.x.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffx : udiffx];
}
}
#if HAS_Y_AXIS
if (shaping.y.ena) {
shaping.y.d_zi[shaping.zi_idx] = traj.y[makeVector_batchIdx];
traj.y[makeVector_batchIdx] *= shaping.y.Ai[0];
for (uint32_t i = 1U; i <= shaping.y.max_i; i++) {
const uint32_t udiffy = shaping.zi_idx - shaping.y.Ni[i];
traj.y[makeVector_batchIdx] += shaping.y.Ai[i] * shaping.y.d_zi[shaping.y.Ni[i] > shaping.zi_idx ? (FTM_ZMAX) + udiffy : udiffy];
}
}
#endif
if (++shaping.zi_idx == (FTM_ZMAX)) shaping.zi_idx = 0;
}
#endif
#endif
// Filled up the queue with regular and shaped steps
if (++makeVector_batchIdx == FTM_WINDOW_SIZE) {
makeVector_batchIdx = last_batchIdx;
makeVector_batchIdx = BATCH_SIDX_IN_WINDOW;
batchRdy = true;
}
if (++makeVector_idx == max_intervals) {
blockProcDn = true;
blockProcRdy = false;
makeVector_idx = 0;
}

83
Marlin/src/module/ft_motion.h
View File

@ -37,20 +37,20 @@
#endif
typedef struct FTConfig {
ftMotionMode_t mode = FTM_DEFAULT_MODE; // Mode / active compensation mode configuration.
bool modeHasShaper() { return WITHIN(mode, 10U, 19U); }
bool active = ENABLED(FTM_IS_DEFAULT_MOTION); // Active (else standard motion)
#if HAS_X_AXIS
ftMotionShaper_t shaper[1 + ENABLED(HAS_Y_AXIS)] = // Shaper type
{ FTM_DEFAULT_SHAPER_X OPTARG(HAS_Y_AXIS, FTM_DEFAULT_SHAPER_Y) };
float baseFreq[1 + ENABLED(HAS_Y_AXIS)] = // Base frequency. [Hz]
{ FTM_SHAPING_DEFAULT_X_FREQ OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) };
float zeta[1 + ENABLED(HAS_Y_AXIS)] = // Damping factor
{ FTM_SHAPING_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_ZETA_Y) };
{ FTM_SHAPING_ZETA_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_ZETA_Y) };
float vtol[1 + ENABLED(HAS_Y_AXIS)] = // Vibration Level
{ FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) };
{ FTM_SHAPING_V_TOL_X OPTARG(HAS_Y_AXIS, FTM_SHAPING_V_TOL_Y) };
#endif
#if HAS_DYNAMIC_FREQ
#if HAS_DYNAMIC_FREQ
dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration.
float dynFreqK[1 + ENABLED(HAS_Y_AXIS)] = { 0.0f }; // Scaling / gain for dynamic frequency. [Hz/mm] or [Hz/g]
#else
@ -72,16 +72,21 @@ class FTMotion {
static bool busy;
static void set_defaults() {
cfg.mode = FTM_DEFAULT_MODE;
cfg.active = ENABLED(FTM_IS_DEFAULT_MOTION);
TERN_(HAS_X_AXIS, cfg.baseFreq[X_AXIS] = FTM_SHAPING_DEFAULT_X_FREQ);
TERN_(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] = FTM_SHAPING_DEFAULT_Y_FREQ);
#if HAS_X_AXIS
cfg.shaper[X_AXIS] = FTM_DEFAULT_SHAPER_X;
cfg.baseFreq[X_AXIS] = FTM_SHAPING_DEFAULT_X_FREQ;
cfg.zeta[X_AXIS] = FTM_SHAPING_ZETA_X;
cfg.vtol[X_AXIS] = FTM_SHAPING_V_TOL_X;
#endif
TERN_(HAS_X_AXIS, cfg.zeta[X_AXIS] = FTM_SHAPING_ZETA_X);
TERN_(HAS_Y_AXIS, cfg.zeta[Y_AXIS] = FTM_SHAPING_ZETA_Y);
TERN_(HAS_X_AXIS, cfg.vtol[X_AXIS] = FTM_SHAPING_V_TOL_X);
TERN_(HAS_Y_AXIS, cfg.vtol[Y_AXIS] = FTM_SHAPING_V_TOL_Y);
#if HAS_Y_AXIS
cfg.shaper[Y_AXIS] = FTM_DEFAULT_SHAPER_Y;
cfg.baseFreq[Y_AXIS] = FTM_SHAPING_DEFAULT_Y_FREQ;
cfg.zeta[Y_AXIS] = FTM_SHAPING_ZETA_Y;
cfg.vtol[Y_AXIS] = FTM_SHAPING_V_TOL_Y;
#endif
#if HAS_DYNAMIC_FREQ
cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE;
@ -93,10 +98,7 @@ class FTMotion {
cfg.linearAdvK = FTM_LINEAR_ADV_DEFAULT_K;
#endif
#if HAS_X_AXIS
refreshShapingN();
updateShapingA();
#endif
TERN_(HAS_X_AXIS, update_shaping_params());
reset();
}
@ -107,43 +109,24 @@ class FTMotion {
static bool sts_stepperBusy; // The stepper buffer has items and is in use.
static millis_t axis_pos_move_end_ti[NUM_AXIS_ENUMS],
axis_neg_move_end_ti[NUM_AXIS_ENUMS];
// Public methods
static void init();
static void startBlockProc(); // Set controller states to begin processing a block.
static bool getBlockProcDn() { return blockProcDn; } // Return true if the controller no longer needs the current block.
static void runoutBlock(); // Move any free data points to the stepper buffer even if a full batch isn't ready.
static void loop(); // Controller main, to be invoked from non-isr task.
#if HAS_X_AXIS
// Refresh the gains used by shaping functions.
// To be called on init or mode or zeta change.
static void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol);
// Refresh the indices used by shaping functions.
// To be called when frequencies change.
static void updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), float zeta[]=cfg.zeta);
static void refreshShapingN() { updateShapingN(cfg.baseFreq[X_AXIS] OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS])); }
// Refresh gains and indices used by shaping functions.
static void update_shaping_params(void);
#endif
static void reset(); // Reset all states of the fixed time conversion to defaults.
static bool axis_moving_pos(const AxisEnum axis) { return !ELAPSED(millis(), axis_pos_move_end_ti[axis]); }
static bool axis_moving_neg(const AxisEnum axis) { return !ELAPSED(millis(), axis_neg_move_end_ti[axis]); }
private:
static xyze_trajectory_t traj;
static xyze_trajectoryMod_t trajMod;
static bool blockProcRdy, blockProcRdy_z1, blockProcDn;
static bool blockProcRdy;
static bool batchRdy, batchRdyForInterp;
static bool runoutEna;
static bool blockDataIsRunout;
// Trapezoid data variables.
static xyze_pos_t startPosn, // (mm) Start position of block
@ -160,19 +143,12 @@ class FTMotion {
static constexpr uint32_t PROP_BATCHES = CEIL(FTM_WINDOW_SIZE/FTM_BATCH_SIZE) - 1; // Number of batches needed to propagate the current trajectory to the stepper.
#define _DIVCEIL(A,B) (((A) + (B) - 1) / (B))
static constexpr uint32_t _ftm_ratio = TERN(FTM_UNIFIED_BWS, 2, _DIVCEIL(FTM_WINDOW_SIZE, FTM_BATCH_SIZE)),
shaper_intervals = (FTM_BATCH_SIZE) * _DIVCEIL(FTM_ZMAX, FTM_BATCH_SIZE),
min_max_intervals = (FTM_BATCH_SIZE) * _ftm_ratio;
// Make vector variables.
static uint32_t makeVector_idx,
makeVector_idx_z1,
makeVector_batchIdx;
// Interpolation variables.
static uint32_t interpIdx,
interpIdx_z1;
static uint32_t interpIdx;
static xyze_long_t steps;
@ -184,21 +160,20 @@ class FTMotion {
float d_zi[FTM_ZMAX] = { 0.0f }; // Data point delay vector.
float Ai[5]; // Shaping gain vector.
uint32_t Ni[5]; // Shaping time index vector.
uint32_t max_i; // Vector length for the selected shaper.
void updateShapingN(const_float_t f, const_float_t df);
void set_axis_shaping_N(const ftMotionShaper_t shaper, const_float_t f, const_float_t zeta); // Sets the gains used by shaping functions.
void set_axis_shaping_A(const ftMotionShaper_t shaper, const_float_t zeta, const_float_t vtol); // Sets the indices used by shaping functions.
} axis_shaping_t;
typedef struct Shaping {
uint32_t zi_idx, // Index of storage in the data point delay vectors.
max_i; // Vector length for the selected shaper.
uint32_t zi_idx; // Index of storage in the data point delay vectors.
axis_shaping_t x;
#if HAS_Y_AXIS
axis_shaping_t y;
#endif
void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol);
} shaping_t;
static shaping_t shaping; // Shaping data
@ -211,6 +186,8 @@ class FTMotion {
#endif
// Private methods
static void discard_planner_block_protected();
static void runoutBlock();
static int32_t stepperCmdBuffItems();
static void loadBlockData(block_t *const current_block);
static void makeVector();

26
Marlin/src/module/ft_types.h
View File

@ -23,18 +23,17 @@
#include "../core/types.h"
typedef enum FXDTICtrlMode : uint8_t {
ftMotionMode_DISABLED = 0, // Standard Motion
ftMotionMode_ENABLED = 1, // Time-Based Motion
ftMotionMode_ZV = 10, // Zero Vibration
ftMotionMode_ZVD = 11, // Zero Vibration and Derivative
ftMotionMode_ZVDD = 12, // Zero Vibration, Derivative, and Double Derivative
ftMotionMode_ZVDDD = 13, // Zero Vibration, Derivative, Double Derivative, and Triple Derivative
ftMotionMode_EI = 14, // Extra-Intensive
ftMotionMode_2HEI = 15, // 2-Hump Extra-Intensive
ftMotionMode_3HEI = 16, // 3-Hump Extra-Intensive
ftMotionMode_MZV = 17 // Mass-based Zero Vibration
} ftMotionMode_t;
typedef enum FXDTICtrlShaper : uint8_t {
ftMotionShaper_NONE = 0, // No compensator
ftMotionShaper_ZV = 1, // Zero Vibration
ftMotionShaper_ZVD = 2, // Zero Vibration and Derivative
ftMotionShaper_ZVDD = 3, // Zero Vibration, Derivative, and Double Derivative
ftMotionShaper_ZVDDD = 4, // Zero Vibration, Derivative, Double Derivative, and Triple Derivative
ftMotionShaper_EI = 5, // Extra-Intensive
ftMotionShaper_2HEI = 6, // 2-Hump Extra-Intensive
ftMotionShaper_3HEI = 7, // 3-Hump Extra-Intensive
ftMotionShaper_MZV = 8 // Modified Zero Vibration
} ftMotionShaper_t;
enum dynFreqMode_t : uint8_t {
dynFreqMode_DISABLED = 0,
@ -42,7 +41,8 @@ enum dynFreqMode_t : uint8_t {
dynFreqMode_MASS_BASED = 2
};
#define IS_EI_MODE(N) WITHIN(N, ftMotionMode_EI, ftMotionMode_3HEI)
#define CMPNSTR_HAS_SHAPER(A) (ftMotion.cfg.shaper[A] != ftMotionShaper_NONE)
#define CMPNSTR_IS_EISHAPER(A) WITHIN(ftMotion.cfg.shaper[A], ftMotionShaper_EI, ftMotionShaper_3HEI)
typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;

10
Marlin/src/module/planner.cpp
View File

@ -1584,7 +1584,7 @@ void Planner::quick_stop() {
// Restart the block delay for the first movement - As the queue was
// forced to empty, there's no risk the ISR will touch this.
delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
TERN_(HAS_WIRED_LCD, clear_block_buffer_runtime()); // Clear the accumulated runtime
@ -1745,7 +1745,7 @@ bool Planner::_buffer_steps(const xyze_long_t &target
// As there are no queued movements, the Stepper ISR will not touch this
// variable, so there is no risk setting this here (but it MUST be done
// before the following line!!)
delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
// Move buffer head
@ -2804,7 +2804,7 @@ void Planner::buffer_sync_block(const BlockFlagBit sync_flag/*=BLOCK_BIT_SYNC_PO
// As there are no queued movements, the Stepper ISR will not touch this
// variable, so there is no risk setting this here (but it MUST be done
// before the following line!!)
delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
block_buffer_head = next_buffer_head;
@ -3097,7 +3097,7 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
// As there are no queued movements, the Stepper ISR will not touch this
// variable, so there is no risk setting this here (but it MUST be done
// before the following line!!)
delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.mode ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
delay_before_delivering = TERN_(FT_MOTION, ftMotion.cfg.active ? BLOCK_DELAY_NONE :) BLOCK_DELAY_FOR_1ST_MOVE;
}
// Move buffer head
@ -3118,7 +3118,7 @@ bool Planner::buffer_line(const xyze_pos_t &cart, const_feedRate_t fr_mm_s
void Planner::set_machine_position_mm(const abce_pos_t &abce) {
// When FT Motion is enabled, call synchronize() here instead of generating a sync block
if (TERN0(FT_MOTION, ftMotion.cfg.mode)) synchronize();
if (TERN0(FT_MOTION, ftMotion.cfg.active)) synchronize();
TERN_(DISTINCT_E_FACTORS, last_extruder = active_extruder);
TERN_(HAS_POSITION_FLOAT, position_float = abce);

137
Marlin/src/module/stepper.cpp
View File

@ -1531,7 +1531,7 @@ void Stepper::isr() {
uint8_t max_loops = 10;
#if ENABLED(FT_MOTION)
const bool using_ftMotion = ftMotion.cfg.mode;
const bool using_ftMotion = ftMotion.cfg.active;
#else
constexpr bool using_ftMotion = false;
#endif
@ -2610,7 +2610,7 @@ hal_timer_t Stepper::block_phase_isr() {
if (cutter.cutter_mode == CUTTER_MODE_DYNAMIC
&& planner.laser_inline.status.isPowered // isPowered flag set on any parsed G1, G2, G3, or G5 move; cleared on any others.
&& current_block // Block may not be available if steps completed (see discard_current_block() above)
&& cutter.last_block_power != current_block->laser.power // Only update if the power changed
&& cutter.last_block_power != current_block->laser.power // Prevent constant update without change
) {
cutter.apply_power(current_block->laser.power);
cutter.last_block_power = current_block->laser.power;
@ -3534,8 +3534,6 @@ void Stepper::report_positions() {
*/
void Stepper::ftMotion_stepper() {
static AxisBits direction_bits{0};
// Check if the buffer is empty.
ftMotion.sts_stepperBusy = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx);
if (!ftMotion.sts_stepperBusy) return;
@ -3553,34 +3551,14 @@ void Stepper::report_positions() {
#define _FTM_STEP(AXIS) TEST(command, FT_BIT_STEP_##AXIS)
#define _FTM_DIR(AXIS) TEST(command, FT_BIT_DIR_##AXIS)
AxisBits axis_step;
axis_step = LOGICAL_AXIS_ARRAY(
TEST(command, FT_BIT_STEP_E),
TEST(command, FT_BIT_STEP_X), TEST(command, FT_BIT_STEP_Y), TEST(command, FT_BIT_STEP_Z),
TEST(command, FT_BIT_STEP_I), TEST(command, FT_BIT_STEP_J), TEST(command, FT_BIT_STEP_K),
TEST(command, FT_BIT_STEP_U), TEST(command, FT_BIT_STEP_V), TEST(command, FT_BIT_STEP_W)
);
/**
* Set bits in axis_did_move for any axes moving in this block,
* clearing the bits at the start of each new segment.
*/
if (TEST(command, FT_BIT_START)) axis_did_move.reset();
direction_bits = LOGICAL_AXIS_ARRAY(
axis_step.e ? TEST(command, FT_BIT_DIR_E) : direction_bits.e,
axis_step.x ? TEST(command, FT_BIT_DIR_X) : direction_bits.x,
axis_step.y ? TEST(command, FT_BIT_DIR_Y) : direction_bits.y,
axis_step.z ? TEST(command, FT_BIT_DIR_Z) : direction_bits.z,
axis_step.i ? TEST(command, FT_BIT_DIR_I) : direction_bits.i,
axis_step.j ? TEST(command, FT_BIT_DIR_J) : direction_bits.j,
axis_step.k ? TEST(command, FT_BIT_DIR_K) : direction_bits.k,
axis_step.u ? TEST(command, FT_BIT_DIR_U) : direction_bits.u,
axis_step.v ? TEST(command, FT_BIT_DIR_V) : direction_bits.v,
axis_step.w ? TEST(command, FT_BIT_DIR_W) : direction_bits.w
);
// Apply directions (which will apply to the entire linear move)
LOGICAL_AXIS_CODE(
E_APPLY_DIR(direction_bits.e, false),
X_APPLY_DIR(direction_bits.x, false), Y_APPLY_DIR(direction_bits.y, false), Z_APPLY_DIR(direction_bits.z, false),
I_APPLY_DIR(direction_bits.i, false), J_APPLY_DIR(direction_bits.j, false), K_APPLY_DIR(direction_bits.k, false),
U_APPLY_DIR(direction_bits.u, false), V_APPLY_DIR(direction_bits.v, false), W_APPLY_DIR(direction_bits.w, false)
);
#define _FTM_AXIS_DID_MOVE(AXIS) axis_did_move.bset(_AXIS(AXIS), _FTM_STEP(AXIS));
LOGICAL_AXIS_MAP(_FTM_AXIS_DID_MOVE);
/**
* Update direction bits for steppers that were stepped by this command.
@ -3588,13 +3566,8 @@ void Stepper::report_positions() {
* when the block was fetched and are not overwritten here.
*/
// Start a step pulse
LOGICAL_AXIS_CODE(
E_APPLY_STEP(axis_step.e, false),
X_APPLY_STEP(axis_step.x, false), Y_APPLY_STEP(axis_step.y, false), Z_APPLY_STEP(axis_step.z, false),
I_APPLY_STEP(axis_step.i, false), J_APPLY_STEP(axis_step.j, false), K_APPLY_STEP(axis_step.k, false),
U_APPLY_STEP(axis_step.u, false), V_APPLY_STEP(axis_step.v, false), W_APPLY_STEP(axis_step.w, false)
);
#define _FTM_SET_DIR(AXIS) if (_FTM_STEP(AXIS)) last_direction_bits.bset(_AXIS(AXIS), _FTM_DIR(AXIS));
LOGICAL_AXIS_MAP(_FTM_SET_DIR);
if (TERN1(FTM_OPTIMIZE_DIR_STATES, last_set_direction != last_direction_bits)) {
// Apply directions (generally applying to the entire linear move)
@ -3619,7 +3592,7 @@ void Stepper::report_positions() {
START_TIMED_PULSE();
// Update step counts
#define _FTM_STEP_COUNT(AXIS) if (axis_step[_AXIS(AXIS)]) count_position[_AXIS(AXIS)] += direction_bits[_AXIS(AXIS)] ? 1 : -1;
#define _FTM_STEP_COUNT(AXIS) if (_FTM_STEP(AXIS)) count_position[_AXIS(AXIS)] += last_direction_bits[_AXIS(AXIS)] ? 1 : -1;
LOGICAL_AXIS_MAP(_FTM_STEP_COUNT);
// Provide EDGE flags for E stepper(s)
@ -3635,10 +3608,10 @@ void Stepper::report_positions() {
// Only wait for axes without edge stepping
const bool any_wait = false LOGICAL_AXIS_GANG(
|| (!e_axis_has_dedge && axis_step.e),
|| (!AXIS_HAS_DEDGE(X) && axis_step.x), || (!AXIS_HAS_DEDGE(Y) && axis_step.y), || (!AXIS_HAS_DEDGE(Z) && axis_step.z),
|| (!AXIS_HAS_DEDGE(I) && axis_step.i), || (!AXIS_HAS_DEDGE(J) && axis_step.j), || (!AXIS_HAS_DEDGE(K) && axis_step.k),
|| (!AXIS_HAS_DEDGE(U) && axis_step.u), || (!AXIS_HAS_DEDGE(V) && axis_step.v), || (!AXIS_HAS_DEDGE(W) && axis_step.w)
|| (!e_axis_has_dedge && _FTM_STEP(E)),
|| (!AXIS_HAS_DEDGE(X) && _FTM_STEP(X)), || (!AXIS_HAS_DEDGE(Y) && _FTM_STEP(Y)), || (!AXIS_HAS_DEDGE(Z) && _FTM_STEP(Z)),
|| (!AXIS_HAS_DEDGE(I) && _FTM_STEP(I)), || (!AXIS_HAS_DEDGE(J) && _FTM_STEP(J)), || (!AXIS_HAS_DEDGE(K) && _FTM_STEP(K)),
|| (!AXIS_HAS_DEDGE(U) && _FTM_STEP(U)), || (!AXIS_HAS_DEDGE(V) && _FTM_STEP(V)), || (!AXIS_HAS_DEDGE(W) && _FTM_STEP(W))
);
// Allow pulses to be registered by stepper drivers
@ -3648,83 +3621,13 @@ void Stepper::report_positions() {
#define _FTM_STEP_STOP(AXIS) AXIS##_APPLY_STEP(!STEP_STATE_##AXIS, false);
LOGICAL_AXIS_MAP(_FTM_STEP_STOP);
// Also handle babystepping here
TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
// Check endstops on every step using axis_did_move as set by every step
// TODO: Update endstop states less frequently to save processing.
// NOTE: endstops.poll is still called at 1KHz by Temperature ISR.
IF_DISABLED(ENDSTOP_INTERRUPTS_FEATURE, if ((bool)axis_did_move) endstops.update());
} // Stepper::ftMotion_stepper
// Called from FTMotion::loop (when !blockProcRdy) which is called from Marlin idle()
void Stepper::ftMotion_blockQueueUpdate() {
if (current_block) {
// If the current block is not done processing, return right away.
// A block is done processing when the command buffer has been
// filled, not necessarily when it's done running.
if (!ftMotion.getBlockProcDn()) return;
planner.release_current_block();
}
// Check the buffer for a new block
current_block = planner.get_current_block();
if (current_block) {
// Sync position, fan power, laser power?
while (current_block->is_sync()) {
#if 0
// TODO: Implement compatible sync blocks with FT Motion commands,
// perhaps by setting a FT_BIT_SYNC flag that holds the current block
// until it is processed by ftMotion_stepper
// Set laser power
#if ENABLED(LASER_POWER_SYNC)
if (cutter.cutter_mode == CUTTER_MODE_CONTINUOUS) {
if (current_block->is_sync_pwr()) {
planner.laser_inline.status.isSyncPower = true;
cutter.apply_power(current_block->laser.power);
}
}
#endif
// Set "fan speeds" for a laser module
#if ENABLED(LASER_SYNCHRONOUS_M106_M107)
if (current_block->is_sync_fan()) planner.sync_fan_speeds(current_block->fan_speed);
#endif
// Set position
if (current_block->is_sync_pos()) _set_position(current_block->position);
#endif
// Done with this block
planner.release_current_block();
// Try to get a new block
if (!(current_block = planner.get_current_block()))
return; // No queued blocks.
}
// Some kinematics track axis motion in HX, HY, HZ
#if ANY(CORE_IS_XY, CORE_IS_XZ, MARKFORGED_XY, MARKFORGED_YX)
last_direction_bits.hx = current_block->direction_bits.hx;
#endif
#if ANY(CORE_IS_XY, CORE_IS_YZ, MARKFORGED_XY, MARKFORGED_YX)
last_direction_bits.hy = current_block->direction_bits.hy;
#endif
#if ANY(CORE_IS_XZ, CORE_IS_YZ)
last_direction_bits.hz = current_block->direction_bits.hz;
#endif
ftMotion.startBlockProc();
return;
}
ftMotion.runoutBlock();
} // Stepper::ftMotion_blockQueueUpdate()
#endif // FT_MOTION
#if ENABLED(BABYSTEPPING)

2
Marlin/src/module/stepper.h
View File

@ -668,8 +668,6 @@ class Stepper {
}
#if ENABLED(FT_MOTION)
// Manage the planner
static void ftMotion_blockQueueUpdate();
// Set current position in steps when reset flag is set in M493 and planner already synchronized
static void ftMotion_syncPosition();
#endif