0x90/Marlin
12
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

🐛⚡️ FT_MOTION improvements (#26074)

Browse source 
Co-Authored-By: Scott Lahteine <thinkyhead@users.noreply.github.com>
This commit is contained in:
narno2202 2023-12-20 02:56:47 +01:00 committed by Scott Lahteine
parent 0ede16cd5a
commit 67d7562609
16 changed files with 652 additions and 643 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

82
Marlin/Configuration_adv.h
View file

@ -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!).
#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:
// 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.
// ZV : FTM_RATIO / 2
// ZVD, MZV : FTM_RATIO
// 2HEI : FTM_RATIO * 3 / 2
// 3HEI : FTM_RATIO * 2
#endif
/**

4
Marlin/src/gcode/bedlevel/G35.cpp
View file

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

1
Marlin/src/gcode/bedlevel/abl/G29.cpp
View file

@ -225,6 +225,7 @@ public:
* 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

1
Marlin/src/gcode/bedlevel/mbl/G29.cpp
View file

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

1
Marlin/src/gcode/calibrate/G34_M422.cpp
View file

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

111
Marlin/src/gcode/feature/ft_motion/M493.cpp
View file

@ -26,6 +26,7 @@
#include "../../gcode.h"
#include "../../../module/ft_motion.h"
#include "../../../module/stepper.h"
void say_shaping() {
// FT Enabled
@ -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;
@ -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:
@ -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;
}
}
@ -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");
}
@ -216,22 +218,16 @@ void GcodeSuite::M493() {
if (ftMotion.cfg.modeHasShaper()) {
const dynFreqMode_t val = dynFreqMode_t(parser.value_byte());
switch (val) {
#if HAS_DYNAMIC_FREQ_MM
case dynFreqMode_Z_BASED:
#endif
#if HAS_DYNAMIC_FREQ_G
case dynFreqMode_MASS_BASED:
#endif
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
default:
SERIAL_ECHOLNPGM("?Invalid Dynamic Frequency Mode [D] value.");
break;
@ -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
@ -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
Marlin/src/gcode/probe/G38.cpp
View file

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

2
Marlin/src/inc/Conditionals_adv.h
View file

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

6
Marlin/src/lcd/language/language_en.h
View file

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

23
Marlin/src/lcd/menu/menu_motion.cpp
View file

@ -342,6 +342,8 @@ void menu_move() {
#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); });
@ -384,6 +386,8 @@ void menu_move() {
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;
@ -416,10 +420,21 @@ void menu_move() {
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
EDIT_ITEM_FAST(float42_52, MSG_FTM_ZETA, &c.zeta, 0.0f, 1.0f, ftMotion.refreshShapingN);
#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);
#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 (WITHIN(c.mode, ftMotionMode_EI, ftMotionMode_3HEI))
EDIT_ITEM_FAST(float42_52, MSG_FTM_VTOL, &c.vtol, 0.0f, 1.0f, ftMotion.refreshShapingN);
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);
@ -437,7 +452,7 @@ void menu_move() {
#if HAS_EXTRUDERS
EDIT_ITEM(bool, MSG_LINEAR_ADVANCE, &c.linearAdvEna);
EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10);
if (c.linearAdvEna) EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0, 10);
#endif
END_MENU();

665
Marlin/src/module/ft_motion.cpp
View file

@ -32,6 +32,8 @@ 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.");
@ -52,9 +54,7 @@ FTMotion ftMotion;
ft_config_t FTMotion::cfg;
bool FTMotion::busy; // = false
ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of stepper commands.
hal_timer_t FTMotion::stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Buffer of the stepper command timing.
uint8_t FTMotion::stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE] = {0U}; // Buffer of whether DIR needs to be updated.
ft_command_t FTMotion::stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE] = {0U}; // Stepper commands buffer.
uint32_t FTMotion::stepperCmdBuff_produceIdx = 0, // Index of next stepper command write to the buffer.
FTMotion::stepperCmdBuff_consumeIdx = 0; // Index of next stepper command read from the buffer.
@ -64,8 +64,8 @@ bool FTMotion::sts_stepperBusy = false; // The stepper buffer has items
// NOTE: These are sized for Ulendo FBS use.
xyze_trajectory_t FTMotion::traj; // = {0.0f} Storage for fixed-time-based trajectory.
xyze_trajectoryMod_t FTMotion::trajMod; // = {0.0f} Storage for modified fixed-time-based trajectory.
xyze_trajectoryWin_t FTMotion::trajWin; // = {0.0f} Storage for fixed time trajectory window.
block_t* FTMotion::current_block_cpy = nullptr; // Pointer to current block being processed.
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.
@ -76,7 +76,7 @@ bool FTMotion::batchRdy = false; // Indicates a batch of the fixe
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::runout = false; // Indicates if runout is in progress.
bool FTMotion::blockDataIsRunout = false; // Indicates the last loaded block variables are for a runout.
// Trapezoid data variables.
xyze_pos_t FTMotion::startPosn, // (mm) Start position of block
@ -98,15 +98,13 @@ 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 = FTM_BATCH_SIZE; // Index of fixed time trajectory generation within the batch.
FTMotion::makeVector_batchIdx = 0; // Index of fixed time trajectory generation within the batch.
// Interpolation variables.
xyze_long_t FTMotion::steps = { 0 }; // Step count accumulator.
xyze_stepDir_t FTMotion::dirState = LOGICAL_AXIS_ARRAY_1(stepDirState_NOT_SET); // Memory of the currently set step direction of the axis.
uint32_t FTMotion::interpIdx = 0, // Index of current data point being interpolated.
FTMotion::interpIdx_z1 = 0; // Storage for the previously calculated index above.
hal_timer_t FTMotion::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the next step time (in ticks).
// Shaping variables.
#if HAS_X_AXIS
@ -125,8 +123,6 @@ hal_timer_t FTMotion::nextStepTicks = FTM_MIN_TICKS; // Accumulator for the nex
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);
//-----------------------------------------------------------------//
// Function definitions.
//-----------------------------------------------------------------//
@ -134,8 +130,7 @@ constexpr uint32_t last_batchIdx = (FTM_WINDOW_SIZE) - (FTM_BATCH_SIZE);
// Public functions.
// Sets controller states to begin processing a block.
void FTMotion::startBlockProc(block_t * const current_block) {
current_block_cpy = current_block;
void FTMotion::startBlockProc() {
blockProcRdy = true;
blockProcDn = false;
runoutEna = true;
@ -144,30 +139,16 @@ void FTMotion::startBlockProc(block_t * const current_block) {
// Move any free data points to the stepper buffer even if a full batch isn't ready.
void FTMotion::runoutBlock() {
if (runoutEna && !batchRdy) { // If the window is full already (block intervals was a multiple of
// the batch size), or runout is not enabled, no runout is needed.
// Fill out the trajectory window with the last position calculated.
if (makeVector_batchIdx > last_batchIdx)
for (uint32_t i = makeVector_batchIdx; i < (FTM_WINDOW_SIZE); i++) {
LOGICAL_AXIS_CODE(
traj.e[i] = traj.e[makeVector_batchIdx - 1],
traj.x[i] = traj.x[makeVector_batchIdx - 1],
traj.y[i] = traj.y[makeVector_batchIdx - 1],
traj.z[i] = traj.z[makeVector_batchIdx - 1],
traj.i[i] = traj.i[makeVector_batchIdx - 1],
traj.j[i] = traj.j[makeVector_batchIdx - 1],
traj.k[i] = traj.k[makeVector_batchIdx - 1],
traj.u[i] = traj.u[makeVector_batchIdx - 1],
traj.v[i] = traj.v[makeVector_batchIdx - 1],
traj.w[i] = traj.w[makeVector_batchIdx - 1]
);
}
if (!runoutEna) return;
makeVector_batchIdx = last_batchIdx;
batchRdy = true;
runout = true;
}
runoutEna = false;
startPosn = endPosn_prevBlock;
ratio.reset();
max_intervals = cfg.modeHasShaper() ? shaper_intervals : 0;
if (max_intervals <= TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, min_max_intervals - (FTM_BATCH_SIZE))) max_intervals = min_max_intervals;
max_intervals += TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_WINDOW_SIZE) - makeVector_batchIdx;
blockProcRdy = blockDataIsRunout = true;
runoutEna = blockProcDn = false;
}
// Controller main, to be invoked from non-isr task.
@ -184,81 +165,63 @@ void FTMotion::loop() {
if (sts_stepperBusy) return; // Wait until motion buffers are emptied
reset();
blockProcDn = true; // Set queueing to look for next block.
runoutEna = false; // Disabling running out this block, since we want to halt the motion.
stepper.abort_current_block = false; // Abort finished.
}
// Planner processing and block conversion.
if (!blockProcRdy && !runout) stepper.ftMotion_BlockQueueUpdate();
if (!blockProcRdy) stepper.ftMotion_blockQueueUpdate();
if (blockProcRdy) {
if (!blockProcRdy_z1) loadBlockData(current_block_cpy); // One-shot.
if (!blockProcRdy_z1) { // One-shot.
if (!blockDataIsRunout) loadBlockData(stepper.current_block);
else blockDataIsRunout = false;
}
while (!blockProcDn && !batchRdy && (makeVector_idx - makeVector_idx_z1 < (FTM_POINTS_PER_LOOP)))
makeVector();
}
if (runout && !batchRdy) { // The lower half of the window has been runout.
// Runout the upper half of the window: the upper half has been shifted into the lower
// half. Fill out the upper half so another batch can be processed.
for (uint32_t i = last_batchIdx; i < (FTM_WINDOW_SIZE) - 1; i++) {
LOGICAL_AXIS_CODE(
traj.e[i] = traj.e[(FTM_WINDOW_SIZE) - 1],
traj.x[i] = traj.x[(FTM_WINDOW_SIZE) - 1],
traj.y[i] = traj.y[(FTM_WINDOW_SIZE) - 1],
traj.z[i] = traj.z[(FTM_WINDOW_SIZE) - 1],
traj.i[i] = traj.i[(FTM_WINDOW_SIZE) - 1],
traj.j[i] = traj.j[(FTM_WINDOW_SIZE) - 1],
traj.k[i] = traj.k[(FTM_WINDOW_SIZE) - 1],
traj.u[i] = traj.u[(FTM_WINDOW_SIZE) - 1],
traj.v[i] = traj.v[(FTM_WINDOW_SIZE) - 1],
traj.w[i] = traj.w[(FTM_WINDOW_SIZE) - 1]
);
}
batchRdy = true;
runout = false;
}
// FBS / post processing.
if (batchRdy && !batchRdyForInterp) {
// Call Ulendo FBS here.
// Copy the uncompensated vectors. (XY done, other axes uncompensated)
#if ENABLED(FTM_UNIFIED_BWS)
trajMod = traj; // Copy the uncompensated vectors.
traj = trajWin; // Move the window to traj
#else
// Copy the uncompensated vectors.
#define TCOPY(A) memcpy(trajMod.A, traj.A, sizeof(trajMod.A))
LOGICAL_AXIS_CODE(
memcpy(trajMod.e, &traj.e[FTM_BATCH_SIZE], sizeof(trajMod.e)),
memcpy(trajMod.x, &traj.x[FTM_BATCH_SIZE], sizeof(trajMod.x)),
memcpy(trajMod.y, &traj.y[FTM_BATCH_SIZE], sizeof(trajMod.y)),
memcpy(trajMod.z, &traj.z[FTM_BATCH_SIZE], sizeof(trajMod.z)),
memcpy(trajMod.i, &traj.i[FTM_BATCH_SIZE], sizeof(trajMod.i)),
memcpy(trajMod.j, &traj.j[FTM_BATCH_SIZE], sizeof(trajMod.j)),
memcpy(trajMod.k, &traj.k[FTM_BATCH_SIZE], sizeof(trajMod.k)),
memcpy(trajMod.u, &traj.u[FTM_BATCH_SIZE], sizeof(trajMod.u)),
memcpy(trajMod.v, &traj.v[FTM_BATCH_SIZE], sizeof(trajMod.v)),
memcpy(trajMod.w, &traj.w[FTM_BATCH_SIZE], sizeof(trajMod.w))
TCOPY(e),
TCOPY(x), TCOPY(y), TCOPY(z),
TCOPY(i), TCOPY(j), TCOPY(k),
TCOPY(u), TCOPY(v), TCOPY(w)
);
// Shift the time series back in the window for (shaped) X and Y
TERN_(HAS_X_AXIS, memcpy(traj.x, &traj.x[FTM_BATCH_SIZE], sizeof(traj.x) / 2));
TERN_(HAS_Y_AXIS, memcpy(traj.y, &traj.y[FTM_BATCH_SIZE], sizeof(traj.y) / 2));
// Z...W and E Disabled! Uncompensated so the lower half is not used.
//TERN_(HAS_Z_AXIS, memcpy(&traj.z[0], &traj.z[FTM_BATCH_SIZE], sizeof(traj.z) / 2));
// Shift the time series back in the window
#define TSHIFT(A) memcpy(traj.A, trajWin.A, sizeof(trajWin.A))
LOGICAL_AXIS_CODE(
TSHIFT(e),
TSHIFT(x), TSHIFT(y), TSHIFT(z),
TSHIFT(i), TSHIFT(j), TSHIFT(k),
TSHIFT(u), TSHIFT(v), TSHIFT(w)
);
#endif
// ... data is ready in trajMod.
batchRdyForInterp = true;
batchRdy = false; // Clear so that makeVector() may resume generating points.
} // if (batchRdy && !batchRdyForInterp)
batchRdy = false; // Clear so makeVector() can resume generating points.
}
// 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) )
&& ( interpIdx - interpIdx_z1 < (FTM_STEPS_PER_LOOP) )
) {
convertToSteps(interpIdx);
if (++interpIdx == FTM_BATCH_SIZE) {
if (++interpIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE)) {
batchRdyForInterp = false;
interpIdx = 0;
}
@ -277,76 +240,153 @@ void FTMotion::loop() {
// Refresh the gains used by shaping functions.
// To be called on init or mode or zeta change.
void FTMotion::Shaping::updateShapingA(const_float_t zeta/*=cfg.zeta*/, const_float_t vtol/*=cfg.vtol*/) {
void FTMotion::Shaping::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) {
const float K = exp(-zeta * M_PI / sqrt(1.0f - sq(zeta))),
K2 = sq(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 (cfg.mode) {
case ftMotionMode_ZV:
max_i = 1U;
x.Ai[0] = 1.0f / (1.0f + K);
x.Ai[1] = x.Ai[0] * K;
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;
break;
case ftMotionMode_ZVD:
max_i = 2U;
x.Ai[0] = 1.0f / ( 1.0f + 2.0f * K + K2 );
x.Ai[1] = x.Ai[0] * 2.0f * K;
x.Ai[2] = x.Ai[0] * K2;
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;
break;
case ftMotionMode_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);
break;
case ftMotionMode_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);
break;
case ftMotionMode_EI: {
max_i = 2U;
x.Ai[0] = 0.25f * (1.0f + vtol);
x.Ai[1] = 0.50f * (1.0f - vtol) * K;
x.Ai[2] = x.Ai[0] * K2;
const float A_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2]);
for (uint32_t i = 0U; i < 3U; i++) { x.Ai[i] *= A_adj; }
} break;
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;
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]);
for (uint32_t i = 0U; i < 3U; i++) {
x.Ai[i] *= X_adj;
y.Ai[i] *= Y_adj;
}
}
break;
case ftMotionMode_2HEI: {
max_i = 3U;
const float vtol2 = sq(vtol);
const float X = pow(vtol2 * (sqrt(1.0f - vtol2) + 1.0f), 1.0f / 3.0f);
x.Ai[0] = ( 3.0f * sq(X) + 2.0f * X + 3.0f * vtol2 ) / (16.0f * X);
x.Ai[1] = ( 0.5f - x.Ai[0] ) * K;
x.Ai[2] = x.Ai[1] * K;
x.Ai[3] = x.Ai[0] * cu(K);
const float A_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3]);
for (uint32_t i = 0U; i < 4U; i++) { x.Ai[i] *= A_adj; }
} break;
const float vtolx2 = sq(vtol[0]);
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);
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]);
for (uint32_t i = 0U; i < 4U; i++) {
x.Ai[i] *= X_adj;
y.Ai[i] *= Y_adj;
}
}
break;
case ftMotionMode_3HEI: {
max_i = 4U;
x.Ai[0] = 0.0625f * ( 1.0f + 3.0f * vtol + 2.0f * sqrt( 2.0f * ( vtol + 1.0f ) * vtol ) );
x.Ai[1] = 0.25f * ( 1.0f - vtol ) * K;
x.Ai[2] = ( 0.5f * ( 1.0f + vtol ) - 2.0f * x.Ai[0] ) * K2;
x.Ai[3] = x.Ai[1] * K2;
x.Ai[4] = x.Ai[0] * sq(K2);
const float A_adj = 1.0f / (x.Ai[0] + x.Ai[1] + x.Ai[2] + x.Ai[3] + x.Ai[4]);
for (uint32_t i = 0U; i < 5U; i++) { x.Ai[i] *= A_adj; }
} break;
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);
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]);
for (uint32_t i = 0U; i < 5U; i++) {
x.Ai[i] *= X_adj;
y.Ai[i] *= Y_adj;
}
}
break;
case ftMotionMode_MZV: {
max_i = 2U;
const float B = 1.4142135623730950488016887242097f * K;
x.Ai[0] = 1.0f / (1.0f + B + K2);
x.Ai[1] = x.Ai[0] * B;
x.Ai[2] = x.Ai[0] * K2;
} break;
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;
}
break;
default:
for (uint32_t i = 0U; i < 5U; i++) x.Ai[i] = 0.0f;
ZERO(x.Ai);
ZERO(y.Ai);
max_i = 0;
}
#if HAS_Y_AXIS
memcpy(y.Ai, x.Ai, sizeof(x.Ai)); // For now, zeta and vtol are shared across x and y.
#endif
}
void FTMotion::updateShapingA(const_float_t zeta/*=cfg.zeta*/, const_float_t vtol/*=cfg.vtol*/) {
void FTMotion::updateShapingA(float zeta[]/*=cfg.zeta*/, float vtol[]/*=cfg.vtol*/) {
shaping.updateShapingA(zeta, vtol);
}
@ -364,11 +404,13 @@ void FTMotion::loop() {
Ni[1] = round((0.5f / f / df) * (FTM_FS));
Ni[2] = Ni[1] + Ni[1];
break;
case ftMotionMode_ZVDD:
case ftMotionMode_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:
Ni[1] = round((0.5f / f / df) * (FTM_FS));
Ni[2] = Ni[1] + Ni[1];
@ -383,10 +425,14 @@ void FTMotion::loop() {
}
}
void FTMotion::updateShapingN(const_float_t xf OPTARG(HAS_Y_AXIS, const_float_t yf), const_float_t zeta/*=cfg.zeta*/) {
const float df = sqrt(1.0f - sq(zeta));
shaping.x.updateShapingN(xf, df);
TERN_(HAS_Y_AXIS, shaping.y.updateShapingN(yf, df));
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);
#if HAS_Y_AXIS
const float ydf = sqrt(1.0f - sq(zeta[1]));
shaping.y.updateShapingN(yf, ydf);
#endif
}
#endif // HAS_X_AXIS
@ -396,27 +442,24 @@ void FTMotion::reset() {
stepperCmdBuff_produceIdx = stepperCmdBuff_consumeIdx = 0;
traj.reset(); // Reset trajectory history
trajMod.reset(); // Reset modified trajectory history
traj.reset();
trajWin.reset();
blockProcRdy = blockProcRdy_z1 = blockProcDn = false;
batchRdy = batchRdyForInterp = false;
runoutEna = false;
runout = false;
endPosn_prevBlock.reset();
makeVector_idx = makeVector_idx_z1 = 0;
makeVector_batchIdx = FTM_BATCH_SIZE;
makeVector_batchIdx = 0;
steps.reset();
interpIdx = interpIdx_z1 = 0;
dirState = LOGICAL_AXIS_ARRAY_1(stepDirState_NOT_SET);
nextStepTicks = FTM_MIN_TICKS;
#if HAS_X_AXIS
for (uint32_t i = 0U; i < (FTM_ZMAX); i++)
shaping.x.d_zi[i] = TERN_(HAS_Y_AXIS, shaping.y.d_zi[i] =) 0.0f;
ZERO(shaping.x.d_zi);
TERN_(HAS_Y_AXIS, ZERO(shaping.y.d_zi));
shaping.zi_idx = 0;
#endif
@ -445,40 +488,26 @@ void FTMotion::loadBlockData(block_t * const current_block) {
const float totalLength = current_block->millimeters,
oneOverLength = 1.0f / totalLength;
const AxisBits direction = current_block->direction_bits;
startPosn = endPosn_prevBlock;
xyze_pos_t moveDist = LOGICAL_AXIS_ARRAY(
current_block->steps.e / planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)],
current_block->steps.x / planner.settings.axis_steps_per_mm[X_AXIS],
current_block->steps.y / planner.settings.axis_steps_per_mm[Y_AXIS],
current_block->steps.z / planner.settings.axis_steps_per_mm[Z_AXIS],
current_block->steps.i / planner.settings.axis_steps_per_mm[I_AXIS],
current_block->steps.j / planner.settings.axis_steps_per_mm[J_AXIS],
current_block->steps.k / planner.settings.axis_steps_per_mm[K_AXIS],
current_block->steps.u / planner.settings.axis_steps_per_mm[U_AXIS],
current_block->steps.v / planner.settings.axis_steps_per_mm[V_AXIS],
current_block->steps.w / planner.settings.axis_steps_per_mm[W_AXIS]
);
LOGICAL_AXIS_CODE(
if (!direction.e) moveDist.e *= -1.0f,
if (!direction.x) moveDist.x *= -1.0f,
if (!direction.y) moveDist.y *= -1.0f,
if (!direction.z) moveDist.z *= -1.0f,
if (!direction.i) moveDist.i *= -1.0f,
if (!direction.j) moveDist.j *= -1.0f,
if (!direction.k) moveDist.k *= -1.0f,
if (!direction.u) moveDist.u *= -1.0f,
if (!direction.v) moveDist.v *= -1.0f,
if (!direction.w) moveDist.w *= -1.0f
current_block->steps.e * planner.mm_per_step[E_AXIS_N(current_block->extruder)] * (current_block->direction_bits.e ? 1 : -1),
current_block->steps.x * planner.mm_per_step[X_AXIS] * (current_block->direction_bits.x ? 1 : -1),
current_block->steps.y * planner.mm_per_step[Y_AXIS] * (current_block->direction_bits.y ? 1 : -1),
current_block->steps.z * planner.mm_per_step[Z_AXIS] * (current_block->direction_bits.z ? 1 : -1),
current_block->steps.i * planner.mm_per_step[I_AXIS] * (current_block->direction_bits.i ? 1 : -1),
current_block->steps.j * planner.mm_per_step[J_AXIS] * (current_block->direction_bits.j ? 1 : -1),
current_block->steps.k * planner.mm_per_step[K_AXIS] * (current_block->direction_bits.k ? 1 : -1),
current_block->steps.u * planner.mm_per_step[U_AXIS] * (current_block->direction_bits.u ? 1 : -1),
current_block->steps.v * planner.mm_per_step[V_AXIS] * (current_block->direction_bits.v ? 1 : -1),
current_block->steps.w * planner.mm_per_step[W_AXIS] * (current_block->direction_bits.w ? 1 : -1)
);
ratio = moveDist * oneOverLength;
/* Keep for comprehension
const float spm = totalLength / current_block->step_event_count; // (steps/mm) Distance for each step
f_s = spm * current_block->initial_rate; // (steps/s) Start feedrate
const float f_e = spm * current_block->final_rate; // (steps/s) End feedrate
f_s = spm * current_block->initial_rate, // (steps/s) Start feedrate
f_e = spm * current_block->final_rate; // (steps/s) End feedrate
const float a = current_block->acceleration, // (mm/s^2) Same magnitude for acceleration or deceleration
oneby2a = 1.0f / (2.0f * a), // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2
@ -499,6 +528,26 @@ void FTMotion::loadBlockData(block_t * const current_block) {
const float T1 = (F_n - f_s) / a, // (s) Accel Time = difference in feedrate over acceleration
T3 = (F_n - f_e) / a; // (s) Decel Time = difference in feedrate over acceleration
*/
const float spm = totalLength / current_block->step_event_count,
f_s = spm * current_block->initial_rate,
f_e = spm * current_block->final_rate;
const float accel = current_block->acceleration,
oneOverAccel = 1.0f / accel;
float F_n = current_block->nominal_speed;
const float ldiff = totalLength + 0.5f * oneOverAccel * (sq(f_s) + sq(f_e));
float T2 = ldiff / F_n - oneOverAccel * F_n;
if (T2 < 0.0f) {
T2 = 0.0f;
F_n = SQRT(ldiff * accel);
}
const float T1 = (F_n - f_s) * oneOverAccel,
T3 = (F_n - f_e) * oneOverAccel;
N1 = ceil(T1 * (FTM_FS)); // Accel datapoints based on Hz frequency
N2 = ceil(T2 * (FTM_FS)); // Coast
@ -508,16 +557,17 @@ void FTMotion::loadBlockData(block_t * const current_block) {
T2_P = N2 * (FTM_TS), // (s) Coast
T3_P = N3 * (FTM_TS); // (s) Decel
// Calculate the reachable feedrate at the end of the accel phase
// totalLength is the total distance to travel in mm
// f_s is the starting feedrate in mm/s
// f_e is the ending feedrate in mm/s
// T1_P is the time spent accelerating in seconds
// T2_P is the time spent coasting in seconds
// T3_P is the time spent decelerating in seconds
// f_s * T1_P is the distance traveled during the accel phase
// f_e * T3_P is the distance traveled during the decel phase
//
/**
* Calculate the reachable feedrate at the end of the accel phase.
* totalLength is the total distance to travel in mm
* f_s : (mm/s) Starting feedrate
* f_e : (mm/s) Ending feedrate
* T1_P : (sec) Time spent accelerating
* T2_P : (sec) Time spent coasting
* T3_P : (sec) Time spent decelerating
* f_s * T1_P : (mm) Distance traveled during the accel phase
* f_e * T3_P : (mm) Distance traveled during the decel phase
*/
F_P = (2.0f * totalLength - f_s * T1_P - f_e * T3_P) / (T1_P + 2.0f * T2_P + T3_P); // (mm/s) Feedrate at the end of the accel phase
// Calculate the acceleration and deceleration rates
@ -531,8 +581,8 @@ void FTMotion::loadBlockData(block_t * const current_block) {
// Calculate the distance traveled during the decel phase
s_2e = s_1e + F_P * T2_P;
// One less than (Accel + Coasting + Decel) datapoints
max_intervals = N1 + N2 + N3 - 1U;
// Accel + Coasting + Decel datapoints
max_intervals = N1 + N2 + N3;
endPosn_prevBlock += moveDist;
}
@ -545,63 +595,55 @@ void FTMotion::makeVector() {
if (makeVector_idx < N1) {
// Acceleration phase
dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase
dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase since start of block
accel_k = accel_P; // (mm/s^2) Acceleration K factor from Accel phase
}
else if (makeVector_idx >= N1 && makeVector_idx < (N1 + N2)) {
else if (makeVector_idx < (N1 + N2)) {
// Coasting phase
dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase
dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase since start of block
//accel_k = 0.0f;
}
else {
// Deceleration phase
const float tau_ = tau - (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase
dist = s_2e + F_P * tau_ + 0.5f * decel_P * sq(tau_); // (mm) Distance traveled for deceleration phase
tau -= (N1 + N2) * (FTM_TS); // (s) Time since start of decel phase
dist = s_2e + F_P * tau + 0.5f * decel_P * sq(tau); // (mm) Distance traveled for deceleration phase since start of block
accel_k = decel_P; // (mm/s^2) Acceleration K factor from Decel phase
}
NUM_AXIS_CODE(
traj.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist,
traj.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist,
traj.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist,
traj.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist,
traj.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist,
traj.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist,
traj.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist,
traj.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist,
traj.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist
LOGICAL_AXIS_CODE(
trajWin.e[makeVector_batchIdx] = startPosn.e + ratio.e * dist,
trajWin.x[makeVector_batchIdx] = startPosn.x + ratio.x * dist,
trajWin.y[makeVector_batchIdx] = startPosn.y + ratio.y * dist,
trajWin.z[makeVector_batchIdx] = startPosn.z + ratio.z * dist,
trajWin.i[makeVector_batchIdx] = startPosn.i + ratio.i * dist,
trajWin.j[makeVector_batchIdx] = startPosn.j + ratio.j * dist,
trajWin.k[makeVector_batchIdx] = startPosn.k + ratio.k * dist,
trajWin.u[makeVector_batchIdx] = startPosn.u + ratio.u * dist,
trajWin.v[makeVector_batchIdx] = startPosn.v + ratio.v * dist,
trajWin.w[makeVector_batchIdx] = startPosn.w + ratio.w * dist
);
#if HAS_EXTRUDERS
const float new_raw_z1 = startPosn.e + ratio.e * dist;
if (cfg.linearAdvEna) {
float dedt_adj = (new_raw_z1 - e_raw_z1) * (FTM_FS);
float dedt_adj = (trajWin.e[makeVector_batchIdx] - e_raw_z1) * (FTM_FS);
if (ratio.e > 0.0f) dedt_adj += accel_k * cfg.linearAdvK;
e_raw_z1 = trajWin.e[makeVector_batchIdx];
e_advanced_z1 += dedt_adj * (FTM_TS);
traj.e[makeVector_batchIdx] = e_advanced_z1;
e_raw_z1 = new_raw_z1;
}
else {
traj.e[makeVector_batchIdx] = new_raw_z1;
// Alternatively: ed[makeVector_batchIdx] = startPosn.e + (ratio.e * dist) / (N1 + N2 + N3);
trajWin.e[makeVector_batchIdx] = e_advanced_z1;
}
#endif
// Update shaping parameters if needed.
#if HAS_DYNAMIC_FREQ_MM
static float zd_z1 = 0.0f;
#endif
switch (cfg.dynFreqMode) {
#if HAS_DYNAMIC_FREQ_MM
case dynFreqMode_Z_BASED:
if (traj.z[makeVector_batchIdx] != zd_z1) { // Only update if Z changed.
const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * traj.z[makeVector_batchIdx],
yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * traj.z[makeVector_batchIdx];
updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ), _MAX(yf, FTM_MIN_SHAPE_FREQ));
zd_z1 = traj.z[makeVector_batchIdx];
if (trajWin.z[makeVector_batchIdx] != 0.0f) { // Only update if Z changed.
const float xf = cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * trajWin.z[makeVector_batchIdx]
OPTARG(HAS_Y_AXIS, yf = cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * trajWin.z[makeVector_batchIdx]);
updateShapingN(_MAX(xf, FTM_MIN_SHAPE_FREQ) OPTARG(HAS_Y_AXIS, _MAX(yf, FTM_MIN_SHAPE_FREQ)));
}
break;
#endif
@ -610,8 +652,8 @@ 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]) );
updateShapingN( cfg.baseFreq[X_AXIS] + cfg.dynFreqK[X_AXIS] * trajWin.e[makeVector_batchIdx]
OPTARG(HAS_Y_AXIS, cfg.baseFreq[Y_AXIS] + cfg.dynFreqK[Y_AXIS] * trajWin.e[makeVector_batchIdx]) );
break;
#endif
@ -621,18 +663,18 @@ void FTMotion::makeVector() {
// Apply shaping if in mode.
#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];
shaping.x.d_zi[shaping.zi_idx] = trajWin.x[makeVector_batchIdx];
trajWin.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];
shaping.y.d_zi[shaping.zi_idx] = trajWin.y[makeVector_batchIdx];
trajWin.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];
trajWin.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];
trajWin.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;
@ -640,18 +682,38 @@ void FTMotion::makeVector() {
#endif
// Filled up the queue with regular and shaped steps
if (++makeVector_batchIdx == (FTM_WINDOW_SIZE)) {
makeVector_batchIdx = last_batchIdx;
if (++makeVector_batchIdx == TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE))) {
makeVector_batchIdx = 0;
batchRdy = true;
}
if (makeVector_idx == max_intervals) {
if (++makeVector_idx == max_intervals) {
blockProcDn = true;
blockProcRdy = false;
makeVector_idx = 0;
}
else
makeVector_idx++;
}
/**
* Convert to steps
* - Commands are written in a bitmask with step and dir as single bits.
* - Tests for delta are moved outside the loop.
* - Two functions are used for command computation with an array of function pointers.
*/
static void (*command_set[NUM_AXES TERN0(HAS_EXTRUDERS, +1)])(int32_t&, int32_t&, ft_command_t&, int32_t, int32_t);
static void command_set_pos(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) {
if (e < FTM_CTS_COMPARE_VAL) return;
s++;
b |= bd | bs;
e -= FTM_STEPS_PER_UNIT_TIME;
}
static void command_set_neg(int32_t &e, int32_t &s, ft_command_t &b, int32_t bd, int32_t bs) {
if (e > -(FTM_CTS_COMPARE_VAL)) return;
s--;
b |= bs;
e += FTM_STEPS_PER_UNIT_TIME;
}
// Interpolates single data point to stepper commands.
@ -660,154 +722,61 @@ void FTMotion::convertToSteps(const uint32_t idx) {
//#define STEPS_ROUNDING
#if ENABLED(STEPS_ROUNDING)
const xyze_float_t steps_tar = LOGICAL_AXIS_ARRAY(
trajMod.e[idx] * planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)] + (trajMod.e[idx] < 0.0f ? -0.5f : 0.5f), // May be eliminated if guaranteed positive.
trajMod.x[idx] * planner.settings.axis_steps_per_mm[X_AXIS] + (trajMod.x[idx] < 0.0f ? -0.5f : 0.5f),
trajMod.y[idx] * planner.settings.axis_steps_per_mm[Y_AXIS] + (trajMod.y[idx] < 0.0f ? -0.5f : 0.5f),
trajMod.z[idx] * planner.settings.axis_steps_per_mm[Z_AXIS] + (trajMod.z[idx] < 0.0f ? -0.5f : 0.5f),
trajMod.i[idx] * planner.settings.axis_steps_per_mm[I_AXIS] + (trajMod.i[idx] < 0.0f ? -0.5f : 0.5f),
trajMod.j[idx] * planner.settings.axis_steps_per_mm[J_AXIS] + (trajMod.j[idx] < 0.0f ? -0.5f : 0.5f),
trajMod.k[idx] * planner.settings.axis_steps_per_mm[K_AXIS] + (trajMod.k[idx] < 0.0f ? -0.5f : 0.5f),
trajMod.u[idx] * planner.settings.axis_steps_per_mm[U_AXIS] + (trajMod.u[idx] < 0.0f ? -0.5f : 0.5f),
trajMod.v[idx] * planner.settings.axis_steps_per_mm[V_AXIS] + (trajMod.v[idx] < 0.0f ? -0.5f : 0.5f),
trajMod.w[idx] * planner.settings.axis_steps_per_mm[W_AXIS] + (trajMod.w[idx] < 0.0f ? -0.5f : 0.5f),
#define TOSTEPS(A,B) int32_t(trajMod.A[idx] * planner.settings.axis_steps_per_mm[B] + (trajMod.A[idx] < 0.0f ? -0.5f : 0.5f))
const xyze_long_t steps_tar = LOGICAL_AXIS_ARRAY(
TOSTEPS(e, E_AXIS_N(current_block->extruder)), // May be eliminated if guaranteed positive.
TOSTEPS(x, X_AXIS), TOSTEPS(y, Y_AXIS), TOSTEPS(z, Z_AXIS),
TOSTEPS(i, I_AXIS), TOSTEPS(j, J_AXIS), TOSTEPS(k, K_AXIS),
TOSTEPS(u, U_AXIS), TOSTEPS(v, V_AXIS), TOSTEPS(w, W_AXIS)
);
xyze_long_t delta = xyze_long_t(steps_tar) - steps;
//const xyze_long_t delta = LOGICAL_AXIS_ARRAY(
// int32_t(steps_tar.e) - steps.e,
// int32_t(steps_tar.x) - steps.x,
// int32_t(steps_tar.y) - steps.y,
// int32_t(steps_tar.z) - steps.z,
// int32_t(steps_tar.i) - steps.i,
// int32_t(steps_tar.j) - steps.j,
// int32_t(steps_tar.k) - steps.k,
// int32_t(steps_tar.u) - steps.u,
// int32_t(steps_tar.v) - steps.v,
// int32_t(steps_tar.w) - steps.w
//);
xyze_long_t delta = steps_tar - steps;
#else
#define TOSTEPS(A,B) int32_t(trajMod.A[idx] * planner.settings.axis_steps_per_mm[B]) - steps.A
xyze_long_t delta = LOGICAL_AXIS_ARRAY(
int32_t(trajMod.e[idx] * planner.settings.axis_steps_per_mm[E_AXIS_N(current_block->extruder)]) - steps.e,
int32_t(trajMod.x[idx] * planner.settings.axis_steps_per_mm[X_AXIS]) - steps.x,
int32_t(trajMod.y[idx] * planner.settings.axis_steps_per_mm[Y_AXIS]) - steps.y,
int32_t(trajMod.z[idx] * planner.settings.axis_steps_per_mm[Z_AXIS]) - steps.z,
int32_t(trajMod.i[idx] * planner.settings.axis_steps_per_mm[I_AXIS]) - steps.i,
int32_t(trajMod.j[idx] * planner.settings.axis_steps_per_mm[J_AXIS]) - steps.j,
int32_t(trajMod.k[idx] * planner.settings.axis_steps_per_mm[K_AXIS]) - steps.k,
int32_t(trajMod.u[idx] * planner.settings.axis_steps_per_mm[U_AXIS]) - steps.u,
int32_t(trajMod.v[idx] * planner.settings.axis_steps_per_mm[V_AXIS]) - steps.v,
int32_t(trajMod.w[idx] * planner.settings.axis_steps_per_mm[W_AXIS]) - steps.w
TOSTEPS(e, E_AXIS_N(current_block->extruder)),
TOSTEPS(x, X_AXIS), TOSTEPS(y, Y_AXIS), TOSTEPS(z, Z_AXIS),
TOSTEPS(i, I_AXIS), TOSTEPS(j, J_AXIS), TOSTEPS(k, K_AXIS),
TOSTEPS(u, U_AXIS), TOSTEPS(v, V_AXIS), TOSTEPS(w, W_AXIS)
);
#endif
bool any_dirChange = (false
LOGICAL_AXIS_GANG(
|| (delta.e > 0 && dirState.e != stepDirState_POS) || (delta.e < 0 && dirState.e != stepDirState_NEG),
|| (delta.x > 0 && dirState.x != stepDirState_POS) || (delta.x < 0 && dirState.x != stepDirState_NEG),
|| (delta.y > 0 && dirState.y != stepDirState_POS) || (delta.y < 0 && dirState.y != stepDirState_NEG),
|| (delta.z > 0 && dirState.z != stepDirState_POS) || (delta.z < 0 && dirState.z != stepDirState_NEG),
|| (delta.i > 0 && dirState.i != stepDirState_POS) || (delta.i < 0 && dirState.i != stepDirState_NEG),
|| (delta.j > 0 && dirState.j != stepDirState_POS) || (delta.j < 0 && dirState.j != stepDirState_NEG),
|| (delta.k > 0 && dirState.k != stepDirState_POS) || (delta.k < 0 && dirState.k != stepDirState_NEG),
|| (delta.u > 0 && dirState.u != stepDirState_POS) || (delta.u < 0 && dirState.u != stepDirState_NEG),
|| (delta.v > 0 && dirState.v != stepDirState_POS) || (delta.v < 0 && dirState.v != stepDirState_NEG),
|| (delta.w > 0 && dirState.w != stepDirState_POS) || (delta.w < 0 && dirState.w != stepDirState_NEG)
)
LOGICAL_AXIS_CODE(
command_set[E_AXIS_N(current_block->extruder)] = delta.e >= 0 ? command_set_pos : command_set_neg,
command_set[X_AXIS] = delta.x >= 0 ? command_set_pos : command_set_neg,
command_set[Y_AXIS] = delta.y >= 0 ? command_set_pos : command_set_neg,
command_set[Z_AXIS] = delta.z >= 0 ? command_set_pos : command_set_neg,
command_set[I_AXIS] = delta.i >= 0 ? command_set_pos : command_set_neg,
command_set[J_AXIS] = delta.j >= 0 ? command_set_pos : command_set_neg,
command_set[K_AXIS] = delta.k >= 0 ? command_set_pos : command_set_neg,
command_set[U_AXIS] = delta.u >= 0 ? command_set_pos : command_set_neg,
command_set[V_AXIS] = delta.v >= 0 ? command_set_pos : command_set_neg,
command_set[W_AXIS] = delta.w >= 0 ? command_set_pos : command_set_neg
);
for (uint32_t i = 0U; i < (FTM_STEPS_PER_UNIT_TIME); i++) {
// TODO: (?) Since the *delta variables will not change,
// the comparison may be done once before iterating at
// expense of storage and lines of code.
bool anyStep = false;
// Commands are written in a bitmask with step and dir as single bits
auto COMMAND_SET = [&](auto &d, auto &e, auto &s, auto &b, auto bd, auto bs) {
if (d >= 0) {
if (e + d < (FTM_CTS_COMPARE_VAL)) {
e += d;
}
else {
s++;
b |= bd | bs;
e += d - (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
else {
if ((e + d) > -(FTM_CTS_COMPARE_VAL)) {
e += d;
}
else {
s--;
b |= bs;
e += d + (FTM_STEPS_PER_UNIT_TIME);
anyStep = true;
}
}
};
// Init all step/dir bits to 0 (defaulting to reverse/negative motion)
stepperCmdBuff[stepperCmdBuff_produceIdx] = 0;
err_P += delta;
// Set up step/dir bits for all axes
LOGICAL_AXIS_CODE(
COMMAND_SET(delta.e, err_P.e, steps.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E), _BV(FT_BIT_STEP_E)),
COMMAND_SET(delta.x, err_P.x, steps.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X), _BV(FT_BIT_STEP_X)),
COMMAND_SET(delta.y, err_P.y, steps.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y), _BV(FT_BIT_STEP_Y)),
COMMAND_SET(delta.z, err_P.z, steps.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z), _BV(FT_BIT_STEP_Z)),
COMMAND_SET(delta.i, err_P.i, steps.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I), _BV(FT_BIT_STEP_I)),
COMMAND_SET(delta.j, err_P.j, steps.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J), _BV(FT_BIT_STEP_J)),
COMMAND_SET(delta.k, err_P.k, steps.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K), _BV(FT_BIT_STEP_K)),
COMMAND_SET(delta.u, err_P.u, steps.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U), _BV(FT_BIT_STEP_U)),
COMMAND_SET(delta.v, err_P.v, steps.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V), _BV(FT_BIT_STEP_V)),
COMMAND_SET(delta.w, err_P.w, steps.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W), _BV(FT_BIT_STEP_W)),
command_set[E_AXIS_N(current_block->extruder)](err_P.e, steps.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E), _BV(FT_BIT_STEP_E)),
command_set[X_AXIS](err_P.x, steps.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X), _BV(FT_BIT_STEP_X)),
command_set[Y_AXIS](err_P.y, steps.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y), _BV(FT_BIT_STEP_Y)),
command_set[Z_AXIS](err_P.z, steps.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z), _BV(FT_BIT_STEP_Z)),
command_set[I_AXIS](err_P.i, steps.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I), _BV(FT_BIT_STEP_I)),
command_set[J_AXIS](err_P.j, steps.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J), _BV(FT_BIT_STEP_J)),
command_set[K_AXIS](err_P.k, steps.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K), _BV(FT_BIT_STEP_K)),
command_set[U_AXIS](err_P.u, steps.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U), _BV(FT_BIT_STEP_U)),
command_set[V_AXIS](err_P.v, steps.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V), _BV(FT_BIT_STEP_V)),
command_set[W_AXIS](err_P.w, steps.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W), _BV(FT_BIT_STEP_W)),
);
if (!anyStep) {
nextStepTicks += (FTM_MIN_TICKS);
}
else {
stepperCmdBuff_StepRelativeTi[stepperCmdBuff_produceIdx] = nextStepTicks;
const uint8_t dir_index = stepperCmdBuff_produceIdx >> 3,
dir_bit = stepperCmdBuff_produceIdx & 0x7;
if (any_dirChange) {
SBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
auto DIR_SET = [&](auto &d, auto &c, auto &b, auto bd) {
if (d > 0) { b |= bd; c = stepDirState_POS; } else { c = stepDirState_NEG; }
};
LOGICAL_AXIS_CODE(
DIR_SET(delta.e, dirState.e, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_E)),
DIR_SET(delta.x, dirState.x, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_X)),
DIR_SET(delta.y, dirState.y, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Y)),
DIR_SET(delta.z, dirState.z, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_Z)),
DIR_SET(delta.i, dirState.i, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_I)),
DIR_SET(delta.j, dirState.j, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_J)),
DIR_SET(delta.k, dirState.k, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_K)),
DIR_SET(delta.u, dirState.u, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_U)),
DIR_SET(delta.v, dirState.v, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_V)),
DIR_SET(delta.w, dirState.w, stepperCmdBuff[stepperCmdBuff_produceIdx], _BV(FT_BIT_DIR_W)),
);
any_dirChange = false;
}
else { // ...no direction change.
CBI(stepperCmdBuff_ApplyDir[dir_index], dir_bit);
}
if (stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE) - 1)
if (++stepperCmdBuff_produceIdx == (FTM_STEPPERCMD_BUFF_SIZE))
stepperCmdBuff_produceIdx = 0;
else
stepperCmdBuff_produceIdx++;
nextStepTicks = FTM_MIN_TICKS;
}
} // FTM_STEPS_PER_UNIT_TIME loop
}

41
Marlin/src/module/ft_motion.h
View file

@ -26,8 +26,6 @@
#include "ft_types.h"
#define FTM_STEPPERCMD_DIR_SIZE ((FTM_STEPPERCMD_BUFF_SIZE + 7) / 8)
#if HAS_X_AXIS && (HAS_Z_AXIS || HAS_EXTRUDERS)
#define HAS_DYNAMIC_FREQ 1
#if HAS_Z_AXIS
@ -46,10 +44,12 @@ typedef struct FTConfig {
#if HAS_X_AXIS
float baseFreq[1 + ENABLED(HAS_Y_AXIS)] = // Base frequency. [Hz]
{ FTM_SHAPING_DEFAULT_X_FREQ OPTARG(HAS_Y_AXIS, FTM_SHAPING_DEFAULT_Y_FREQ) };
#endif
float zeta = FTM_SHAPING_ZETA; // Damping factor
float vtol = FTM_SHAPING_V_TOL; // Vibration Level
float zeta[1 + ENABLED(HAS_Y_AXIS)] = // Damping factor
{ 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) };
#endif
#if HAS_DYNAMIC_FREQ
dynFreqMode_t dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE; // Dynamic frequency mode configuration.
@ -78,8 +78,11 @@ class FTMotion {
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);
cfg.zeta = FTM_SHAPING_ZETA; // Damping factor
cfg.vtol = FTM_SHAPING_V_TOL; // Vibration Level
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_DYNAMIC_FREQ
cfg.dynFreqMode = FTM_DEFAULT_DYNFREQ_MODE;
@ -100,8 +103,6 @@ class FTMotion {
}
static ft_command_t stepperCmdBuff[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of stepper commands.
static hal_timer_t stepperCmdBuff_StepRelativeTi[FTM_STEPPERCMD_BUFF_SIZE]; // Buffer of the stepper command timing.
static uint8_t stepperCmdBuff_ApplyDir[FTM_STEPPERCMD_DIR_SIZE]; // Buffer of whether DIR needs to be updated.
static uint32_t stepperCmdBuff_produceIdx, // Index of next stepper command write to the buffer.
stepperCmdBuff_consumeIdx; // Index of next stepper command read from the buffer.
@ -110,7 +111,7 @@ class FTMotion {
// Public methods
static void init();
static void startBlockProc(block_t * const current_block); // Set controller states to begin processing a block.
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.
@ -118,28 +119,28 @@ class FTMotion {
#if HAS_X_AXIS
// Refresh the gains used by shaping functions.
// To be called on init or mode or zeta change.
static void updateShapingA(const_float_t zeta=cfg.zeta, const_float_t vtol=cfg.vtol);
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), const_float_t zeta=cfg.zeta);
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])); }
#endif
static void reset(); // Resets all states of the fixed time conversion to defaults.
static void reset(); // Reset all states of the fixed time conversion to defaults.
private:
static xyze_trajectory_t traj;
static xyze_trajectoryMod_t trajMod;
static xyze_trajectoryWin_t trajWin;
static block_t *current_block_cpy;
static bool blockProcRdy, blockProcRdy_z1, blockProcDn;
static bool batchRdy, batchRdyForInterp;
static bool runoutEna;
static bool runout;
static bool blockDataIsRunout;
// Trapezoid data variables.
static xyze_pos_t startPosn, // (mm) Start position of block
@ -154,6 +155,11 @@ class FTMotion {
static uint32_t N1, N2, N3;
static uint32_t max_intervals;
static constexpr uint32_t _ftm_size = TERN(FTM_UNIFIED_BWS, FTM_BW_SIZE, FTM_BATCH_SIZE),
_ftm_wind = TERN(FTM_UNIFIED_BWS, 2, ceil((FTM_WINDOW_SIZE) / _ftm_size)),
shaper_intervals = _ftm_size * ceil((FTM_ZMAX) / _ftm_size),
min_max_intervals = _ftm_size * _ftm_wind;
// Make vector variables.
static uint32_t makeVector_idx,
makeVector_idx_z1,
@ -164,9 +170,6 @@ class FTMotion {
interpIdx_z1;
static xyze_long_t steps;
static xyze_stepDir_t dirState;
static hal_timer_t nextStepTicks;
// Shaping variables.
#if HAS_X_AXIS
@ -188,7 +191,7 @@ class FTMotion {
axis_shaping_t y;
#endif
void updateShapingA(const_float_t zeta=cfg.zeta, const_float_t vtol=cfg.vtol);
void updateShapingA(float zeta[]=cfg.zeta, float vtol[]=cfg.vtol);
} shaping_t;

25
Marlin/src/module/ft_types.h
View file

@ -29,10 +29,12 @@ typedef enum FXDTICtrlMode : uint8_t {
//ftMotionMode_ULENDO_FBS = 2U,
ftMotionMode_ZV = 10U,
ftMotionMode_ZVD = 11U,
ftMotionMode_EI = 12U,
ftMotionMode_2HEI = 13U,
ftMotionMode_3HEI = 14U,
ftMotionMode_MZV = 15U,
ftMotionMode_ZVDD = 12U,
ftMotionMode_ZVDDD = 13U,
ftMotionMode_EI = 14U,
ftMotionMode_2HEI = 15U,
ftMotionMode_3HEI = 16U,
ftMotionMode_MZV = 17U,
//ftMotionMode_DISCTF = 20U
} ftMotionMode_t;
@ -42,16 +44,17 @@ enum dynFreqMode_t : uint8_t {
dynFreqMode_MASS_BASED = 2U
};
enum stepDirState_t : uint8_t {
stepDirState_NOT_SET = 0U,
stepDirState_POS = 1U,
stepDirState_NEG = 2U
};
#define IS_EI_MODE(N) WITHIN(N, ftMotionMode_EI, ftMotionMode_3HEI)
#if ENABLED(FTM_UNIFIED_BWS)
typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectory_t;
typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryMod_t;
typedef struct XYZEarray<float, FTM_BW_SIZE> xyze_trajectoryWin_t;
#else
typedef struct XYZEarray<float, FTM_WINDOW_SIZE> xyze_trajectory_t;
typedef struct XYZEarray<float, FTM_BATCH_SIZE> xyze_trajectoryMod_t;
typedef struct XYZEval<stepDirState_t> xyze_stepDir_t;
typedef struct XYZEarray<float, (FTM_WINDOW_SIZE - FTM_BATCH_SIZE)> xyze_trajectoryWin_t;
#endif
enum {
LIST_N(DOUBLE(LOGICAL_AXES),

19
Marlin/src/module/motion.cpp
View file

@ -33,10 +33,6 @@
#include "../lcd/marlinui.h"
#include "../inc/MarlinConfig.h"
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#endif
#if IS_SCARA
#include "../libs/buzzer.h"
#include "../lcd/marlinui.h"
@ -2095,21 +2091,6 @@ void prepare_line_to_destination() {
void homeaxis(const AxisEnum axis) {
#if ENABLED(FT_MOTION)
// Disable ft-motion for homing
struct OnExit {
ftMotionMode_t oldmm;
OnExit() {
oldmm = ftMotion.cfg.mode;
ftMotion.cfg.mode = ftMotionMode_DISABLED;
}
~OnExit() {
ftMotion.cfg.mode = oldmm;
ftMotion.init();
}
} on_exit;
#endif
#if ANY(MORGAN_SCARA, MP_SCARA)
// Only Z homing (with probe) is permitted
if (axis != Z_AXIS) { BUZZ(100, 880); return; }

222
Marlin/src/module/stepper.cpp
View file

@ -1492,11 +1492,6 @@ void Stepper::isr() {
uint8_t max_loops = 10;
#if ENABLED(FT_MOTION)
static bool ftMotion_stepCmdRdy = false; // Indicates a step command was loaded from the
// buffers and is ready to be output.
static bool ftMotion_applyDir = false; // Indicates the DIR output should be set.
static ft_command_t ftMotion_stepCmd = 0U; // Storage for the step command to be output.
static uint32_t ftMotion_nextAuxISR = 0U; // Storage for the next ISR of the auxilliary tasks.
const bool using_ftMotion = ftMotion.cfg.mode;
#else
constexpr bool using_ftMotion = false;
@ -1508,57 +1503,19 @@ void Stepper::isr() {
// Enable ISRs to reduce USART processing latency
hal.isr_on();
hal_timer_t interval;
hal_timer_t interval = 0;
#if ENABLED(FT_MOTION)
if (using_ftMotion) {
if (!nextMainISR) {
if (abort_current_block) {
ftMotion_stepCmdRdy = false; // If a command was ready, cancel it.
ftMotion.sts_stepperBusy = false; // Set busy false to allow a reset.
nextMainISR = 0.01f * (STEPPER_TIMER_RATE); // Come back in 10 msec.
}
else { // !(abort_current_block)
if (ftMotion_stepCmdRdy) {
ftMotion_stepper(ftMotion_applyDir, ftMotion_stepCmd);
ftMotion_stepCmdRdy = false;
}
// Check if there is data in the buffers.
if (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx) {
ftMotion.sts_stepperBusy = true;
// "Pop" one command from the command buffer.
ftMotion_stepCmd = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];
const uint8_t dir_index = ftMotion.stepperCmdBuff_consumeIdx >> 3,
dir_bit = ftMotion.stepperCmdBuff_consumeIdx & 0x7;
ftMotion_applyDir = TEST(ftMotion.stepperCmdBuff_ApplyDir[dir_index], dir_bit);
nextMainISR = ftMotion.stepperCmdBuff_StepRelativeTi[ftMotion.stepperCmdBuff_consumeIdx];
ftMotion_stepCmdRdy = true;
if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE))
ftMotion.stepperCmdBuff_consumeIdx = 0;
}
else { // Buffer empty.
ftMotion.sts_stepperBusy = false;
nextMainISR = 0.01f * (STEPPER_TIMER_RATE); // Come back in 10 msec.
}
} // !(abort_current_block)
} // if (!nextMainISR)
// Define 2.5 msec task for auxiliary functions.
if (!ftMotion_nextAuxISR) {
nextMainISR = FTM_MIN_TICKS;
ftMotion_stepper();
endstops.update();
TERN_(BABYSTEPPING, if (babystep.has_steps()) babystepping_isr());
ftMotion_refreshAxisDidMove();
ftMotion_nextAuxISR = 0.0025f * (STEPPER_TIMER_RATE);
}
interval = _MIN(nextMainISR, ftMotion_nextAuxISR);
interval = nextMainISR;
nextMainISR -= interval;
ftMotion_nextAuxISR -= interval;
}
#endif
@ -3365,6 +3322,28 @@ void Stepper::set_axis_position(const AxisEnum a, const int32_t &v) {
#endif
}
#if ENABLED(FT_MOTION)
void Stepper::ftMotion_syncPosition() {
//planner.synchronize(); planner already synchronized in M493
#ifdef __AVR__
// Protect the access to the position. Only required for AVR, as
// any 32bit CPU offers atomic access to 32bit variables
const bool was_enabled = suspend();
#endif
// Update stepper positions from the planner
count_position = planner.position;
#ifdef __AVR__
// Reenable Stepper ISR
if (was_enabled) wake_up();
#endif
}
#endif // FT_MOTION
// Signal endstops were triggered - This function can be called from
// an ISR context (Temperature, Stepper or limits ISR), so we must
// be very careful here. If the interrupt being preempted was the
@ -3459,65 +3438,70 @@ void Stepper::report_positions() {
#if ENABLED(FT_MOTION)
// Set stepper I/O for fixed time controller.
void Stepper::ftMotion_stepper(const bool applyDir, const ft_command_t command) {
void Stepper::ftMotion_stepper() {
// Check if the buffer is empty.
ftMotion.sts_stepperBusy = (ftMotion.stepperCmdBuff_produceIdx != ftMotion.stepperCmdBuff_consumeIdx);
if (!ftMotion.sts_stepperBusy) return;
// "Pop" one command from current motion buffer
// Use one byte to restore one stepper command in the format:
// |X_step|X_direction|Y_step|Y_direction|Z_step|Z_direction|E_step|E_direction|
const ft_command_t command = ftMotion.stepperCmdBuff[ftMotion.stepperCmdBuff_consumeIdx];
if (++ftMotion.stepperCmdBuff_consumeIdx == (FTM_STEPPERCMD_BUFF_SIZE)) ftMotion.stepperCmdBuff_consumeIdx = 0U;
if (abort_current_block) return;
USING_TIMED_PULSE();
const xyze_bool_t axis_step = LOGICAL_AXIS_ARRAY(
axis_did_move = 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)
);
last_direction_bits = LOGICAL_AXIS_ARRAY(
axis_did_move.e ? TEST(command, FT_BIT_DIR_E) : last_direction_bits.e,
axis_did_move.x ? TEST(command, FT_BIT_DIR_X) : last_direction_bits.x,
axis_did_move.y ? TEST(command, FT_BIT_DIR_Y) : last_direction_bits.y,
axis_did_move.z ? TEST(command, FT_BIT_DIR_Z) : last_direction_bits.z,
axis_did_move.i ? TEST(command, FT_BIT_DIR_I) : last_direction_bits.i,
axis_did_move.j ? TEST(command, FT_BIT_DIR_J) : last_direction_bits.j,
axis_did_move.k ? TEST(command, FT_BIT_DIR_K) : last_direction_bits.k,
axis_did_move.u ? TEST(command, FT_BIT_DIR_U) : last_direction_bits.u,
axis_did_move.v ? TEST(command, FT_BIT_DIR_V) : last_direction_bits.v,
axis_did_move.w ? TEST(command, FT_BIT_DIR_W) : last_direction_bits.w
);
// Apply directions (which will apply to the entire linear move)
AxisBits axis_dir = last_direction_bits;
if (applyDir) {
axis_dir = LOGICAL_AXIS_ARRAY(
TEST(command, FT_BIT_DIR_E),
TEST(command, FT_BIT_DIR_X), TEST(command, FT_BIT_DIR_Y), TEST(command, FT_BIT_DIR_Z),
TEST(command, FT_BIT_DIR_I), TEST(command, FT_BIT_DIR_J), TEST(command, FT_BIT_DIR_K),
TEST(command, FT_BIT_DIR_U), TEST(command, FT_BIT_DIR_V), TEST(command, FT_BIT_DIR_W)
);
LOGICAL_AXIS_CODE(
E_APPLY_DIR(axis_dir.e, false),
X_APPLY_DIR(axis_dir.x, false), Y_APPLY_DIR(axis_dir.y, false), Z_APPLY_DIR(axis_dir.z, false),
I_APPLY_DIR(axis_dir.i, false), J_APPLY_DIR(axis_dir.j, false), K_APPLY_DIR(axis_dir.k, false),
U_APPLY_DIR(axis_dir.u, false), V_APPLY_DIR(axis_dir.v, false), W_APPLY_DIR(axis_dir.w, false)
E_APPLY_DIR(last_direction_bits.e, false),
X_APPLY_DIR(last_direction_bits.x, false), Y_APPLY_DIR(last_direction_bits.y, false), Z_APPLY_DIR(last_direction_bits.z, false),
I_APPLY_DIR(last_direction_bits.i, false), J_APPLY_DIR(last_direction_bits.j, false), K_APPLY_DIR(last_direction_bits.k, false),
U_APPLY_DIR(last_direction_bits.u, false), V_APPLY_DIR(last_direction_bits.v, false), W_APPLY_DIR(last_direction_bits.w, false)
);
last_direction_bits = axis_dir;
DIR_WAIT_AFTER();
}
// Start a step pulse
LOGICAL_AXIS_CODE(
if (axis_step.e) E_APPLY_STEP(STEP_STATE_E, false),
if (axis_step.x) X_APPLY_STEP(STEP_STATE_X, false), if (axis_step.y) Y_APPLY_STEP(STEP_STATE_Y, false),
if (axis_step.z) Z_APPLY_STEP(STEP_STATE_Z, false), if (axis_step.i) I_APPLY_STEP(STEP_STATE_I, false),
if (axis_step.j) J_APPLY_STEP(STEP_STATE_J, false), if (axis_step.k) K_APPLY_STEP(STEP_STATE_K, false),
if (axis_step.u) U_APPLY_STEP(STEP_STATE_U, false), if (axis_step.v) V_APPLY_STEP(STEP_STATE_V, false),
if (axis_step.w) W_APPLY_STEP(STEP_STATE_W, false)
E_APPLY_STEP(axis_did_move.e, false),
X_APPLY_STEP(axis_did_move.x, false), Y_APPLY_STEP(axis_did_move.y, false), Z_APPLY_STEP(axis_did_move.z, false),
I_APPLY_STEP(axis_did_move.i, false), J_APPLY_STEP(axis_did_move.j, false), K_APPLY_STEP(axis_did_move.k, false),
U_APPLY_STEP(axis_did_move.u, false), V_APPLY_STEP(axis_did_move.v, false), W_APPLY_STEP(axis_did_move.w, false)
);
// Begin waiting for the minimum pulse duration
START_TIMED_PULSE();
// Update axis direction adders
count_direction = LOGICAL_AXIS_ARRAY(
int8_t(axis_dir.e ? 1 : -1),
int8_t(axis_dir.x ? 1 : -1), int8_t(axis_dir.y ? 1 : -1), int8_t(axis_dir.z ? 1 : -1),
int8_t(axis_dir.i ? 1 : -1), int8_t(axis_dir.j ? 1 : -1), int8_t(axis_dir.k ? 1 : -1),
int8_t(axis_dir.u ? 1 : -1), int8_t(axis_dir.v ? 1 : -1), int8_t(axis_dir.w ? 1 : -1)
);
// Update stepper counts - required for various operations
// Update step counts
LOGICAL_AXIS_CODE(
if (axis_step.e) count_position.e += count_direction.e,
if (axis_step.x) count_position.x += count_direction.x, if (axis_step.y) count_position.y += count_direction.y,
if (axis_step.z) count_position.z += count_direction.z, if (axis_step.i) count_position.i += count_direction.i,
if (axis_step.j) count_position.j += count_direction.j, if (axis_step.k) count_position.k += count_direction.k,
if (axis_step.u) count_position.u += count_direction.u, if (axis_step.v) count_position.v += count_direction.v,
if (axis_step.w) count_position.w += count_direction.w
if (axis_did_move.e) count_position.e += last_direction_bits.e ? 1 : -1, if (axis_did_move.x) count_position.x += last_direction_bits.x ? 1 : -1,
if (axis_did_move.y) count_position.y += last_direction_bits.y ? 1 : -1, if (axis_did_move.z) count_position.z += last_direction_bits.z ? 1 : -1,
if (axis_did_move.i) count_position.i += last_direction_bits.i ? 1 : -1, if (axis_did_move.j) count_position.j += last_direction_bits.j ? 1 : -1,
if (axis_did_move.k) count_position.k += last_direction_bits.k ? 1 : -1, if (axis_did_move.u) count_position.u += last_direction_bits.u ? 1 : -1,
if (axis_did_move.v) count_position.v += last_direction_bits.v ? 1 : -1, if (axis_did_move.w) count_position.w += last_direction_bits.w ? 1 : -1
);
#if HAS_EXTRUDERS
@ -3532,10 +3516,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 && axis_did_move.e),
|| (!AXIS_HAS_DEDGE(X) && axis_did_move.x), || (!AXIS_HAS_DEDGE(Y) && axis_did_move.y), || (!AXIS_HAS_DEDGE(Z) && axis_did_move.z),
|| (!AXIS_HAS_DEDGE(I) && axis_did_move.i), || (!AXIS_HAS_DEDGE(J) && axis_did_move.j), || (!AXIS_HAS_DEDGE(K) && axis_did_move.k),
|| (!AXIS_HAS_DEDGE(U) && axis_did_move.u), || (!AXIS_HAS_DEDGE(V) && axis_did_move.v), || (!AXIS_HAS_DEDGE(W) && axis_did_move.w)
);
// Allow pulses to be registered by stepper drivers
@ -3543,80 +3527,46 @@ void Stepper::report_positions() {
// Stop pulses. Axes with DEDGE will do nothing, assuming STEP_STATE_* is HIGH
LOGICAL_AXIS_CODE(
if (axis_step.e) E_APPLY_STEP(!STEP_STATE_E, false),
if (axis_step.x) X_APPLY_STEP(!STEP_STATE_X, false), if (axis_step.y) Y_APPLY_STEP(!STEP_STATE_Y, false),
if (axis_step.z) Z_APPLY_STEP(!STEP_STATE_Z, false), if (axis_step.i) I_APPLY_STEP(!STEP_STATE_I, false),
if (axis_step.j) J_APPLY_STEP(!STEP_STATE_J, false), if (axis_step.k) K_APPLY_STEP(!STEP_STATE_K, false),
if (axis_step.u) U_APPLY_STEP(!STEP_STATE_U, false), if (axis_step.v) V_APPLY_STEP(!STEP_STATE_V, false),
if (axis_step.w) W_APPLY_STEP(!STEP_STATE_W, false)
E_APPLY_STEP(!STEP_STATE_E, false),
X_APPLY_STEP(!STEP_STATE_X, false), Y_APPLY_STEP(!STEP_STATE_Y, false), Z_APPLY_STEP(!STEP_STATE_Z, false),
I_APPLY_STEP(!STEP_STATE_I, false), J_APPLY_STEP(!STEP_STATE_J, false), K_APPLY_STEP(!STEP_STATE_K, false),
U_APPLY_STEP(!STEP_STATE_U, false), V_APPLY_STEP(!STEP_STATE_V, false), W_APPLY_STEP(!STEP_STATE_W, false)
);
} // Stepper::ftMotion_stepper
void Stepper::ftMotion_BlockQueueUpdate() {
void Stepper::ftMotion_blockQueueUpdate() {
if (current_block) {
// If the current block is not done processing, return right away
if (!ftMotion.getBlockProcDn()) return;
axis_did_move.reset();
current_block = nullptr;
discard_current_block();
planner.release_current_block();
}
if (!current_block) { // No current block
// Check the buffer for a new block
current_block = planner.get_current_block();
if (current_block) {
// Sync block? Sync the stepper counts and return
while (current_block->is_sync()) {
if (!(current_block->is_fan_sync() || current_block->is_pwr_sync())) _set_position(current_block->position);
discard_current_block();
TERN_(LASER_FEATURE, if (!(current_block->is_fan_sync() || current_block->is_pwr_sync()))) _set_position(current_block->position);
planner.release_current_block();
// Try to get a new block
if (!(current_block = planner.get_current_block()))
return; // No more queued movements!image.png
return; // No queued blocks.
}
// this is needed by motor_direction() and subsequently bed leveling (somehow)
// update it here, even though it will may be out of sync with step commands
last_direction_bits = current_block->direction_bits;
ftMotion.startBlockProc(current_block);
ftMotion.startBlockProc();
return;
}
else {
ftMotion.runoutBlock();
return; // No queued blocks
}
} // if (!current_block)
} // Stepper::ftMotion_BlockQueueUpdate()
// Debounces the axis move indication to account for potential
// delay between the block information and the stepper commands
void Stepper::ftMotion_refreshAxisDidMove() {
// Set the debounce time in seconds.
#define AXIS_DID_MOVE_DEB 5 // TODO: The debounce time should be calculated if possible,
// or the set conditions should be changed from the block to
// the motion trajectory or motor commands.
AxisBits didmove;
static abce_ulong_t debounce{0};
auto debounce_axis = [&](const AxisEnum axis) {
if (current_block->steps[axis]) debounce[axis] = (AXIS_DID_MOVE_DEB) * 400; // divide by 0.0025f */
if (debounce[axis]) { didmove.bset(axis); debounce[axis]--; }
};
#define _DEBOUNCE(N) debounce_axis(AxisEnum(N));
if (current_block) { REPEAT(LOGICAL_AXES, _DEBOUNCE); }
axis_did_move = didmove;
}
} // Stepper::ftMotion_blockQueueUpdate()
#endif // FT_MOTION

7
Marlin/src/module/stepper.h
View file

@ -655,7 +655,9 @@ class Stepper {
#if ENABLED(FT_MOTION)
// Manage the planner
static void ftMotion_BlockQueueUpdate();
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
#if HAS_ZV_SHAPING
@ -694,8 +696,7 @@ class Stepper {
#endif
#if ENABLED(FT_MOTION)
static void ftMotion_stepper(const bool applyDir, const ft_command_t command);
static void ftMotion_refreshAxisDidMove();
static void ftMotion_stepper();
#endif
};