3D-printing/Marlin
1
0
Fork 0
mirror of https://github.com/MarlinFirmware/Marlin.git synced 2025-12-27 09:59:52 -07:00
Code Issues Projects Releases Packages Wiki Activity Actions 4

⚡️ FTMotion Trajectory: Fix for short blocks (silent curves), add S-curve (#28082)

Browse source
This commit is contained in:
David Buezas 2025-10-02 00:27:47 +02:00 • committed by GitHub
parent ba840750a0
commit eaefa299fb
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
15 changed files with 787 additions and 210 deletions
Download patch file Download diff file Expand all files Collapse all files

9
Marlin/Configuration_adv.h
View file

@ -1191,6 +1191,15 @@
// smoothing acceleration peaks, which may also smooth curved surfaces.
#endif
#define FTM_TRAJECTORY_TYPE TRAPEZOIDAL // Block acceleration profile (TRAPEZOIDAL, POLY5, POLY6)
// TRAPEZOIDAL: Continuous Velocity. Max acceleration is respected.
// POLY5: Like POLY6 with 1.5x but cpu cheaper.
// POLY6: Continuous Acceleration (aka S_CURVE).
// POLY trajectories not only reduce resonances without rounding corners, but also
// reduce extruder strain due to linear advance.
#define FTM_POLY6_ACCELERATION_OVERSHOOT 1.875f // Max acceleration overshoot factor for POLY6 (1.25 to 1.875)
/**
* Advanced configuration
*/

148
Marlin/src/gcode/feature/ft_motion/M494.cpp
View file

@ -21,44 +21,65 @@
*/
#include "../../../inc/MarlinConfigPre.h"
#if ENABLED(FTM_SMOOTHING)
#if ENABLED(FT_MOTION)
#include "../../gcode.h"
#include "../../../module/ft_motion.h"
#include "../../../module/stepper.h"
#include "../../../module/planner.h"
void say_smoothing() {
#if HAS_X_AXIS
SERIAL_ECHOLN(F(" "), C('X'), F(" smoothing time: "), p_float_t(ftMotion.cfg.smoothingTime.X, 3), C('s'));
#endif
#if HAS_Y_AXIS
SERIAL_ECHOLN(F(" "), C('Y'), F(" smoothing time: "), p_float_t(ftMotion.cfg.smoothingTime.Y, 3), C('s'));
#endif
#if HAS_Z_AXIS
SERIAL_ECHOLN(F(" "), C('Z'), F(" smoothing time: "), p_float_t(ftMotion.cfg.smoothingTime.Z, 3), C('s'));
#endif
#if HAS_EXTRUDERS
SERIAL_ECHOLN(F(" "), C('E'), F(" smoothing time: "), p_float_t(ftMotion.cfg.smoothingTime.E, 3), C('s'));
static FSTR_P get_trajectory_type_name() {
switch (ftMotion.getTrajectoryType()) {
default:
case TrajectoryType::TRAPEZOIDAL: return GET_TEXT_F(MSG_FTM_TRAPEZOIDAL);
case TrajectoryType::POLY5: return GET_TEXT_F(MSG_FTM_POLY5);
case TrajectoryType::POLY6: return GET_TEXT_F(MSG_FTM_POLY6);
}
}
void say_ftm_settings() {
SERIAL_ECHOLN(F(" Trajectory: "), get_trajectory_type_name(), C('('), (uint8_t)ftMotion.getTrajectoryType(), C(')'));
const ft_config_t &c = ftMotion.cfg;
if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6)
SERIAL_ECHOLNPGM(" Poly6 Overshoot: ", p_float_t(c.poly6_acceleration_overshoot, 3));
#if ENABLED(FTM_SMOOTHING)
#define _SMOO_REPORT(A) SERIAL_ECHOLN(F(" "), C(IAXIS_CHAR(_AXIS(A))), F(" smoothing time: "), p_float_t(c.smoothingTime.A, 3), C('s'));
CARTES_MAP(_SMOO_REPORT);
#endif
}
void GcodeSuite::M494_report(const bool forReplay/*=true*/) {
TERN_(MARLIN_SMALL_BUILD, return);
report_heading_etc(forReplay, F("FTM Smoothing"));
const ft_config_t &c = ftMotion.cfg;
SERIAL_ECHOLN(F(" M494")
CARTES_COMMA CARTES_PAIRED_LIST(
F(" X"), c.smoothingTime.X, F(" Y"), c.smoothingTime.Y,
F(" Z"), c.smoothingTime.Z, F(" E"), c.smoothingTime.E
)
);
report_heading_etc(forReplay, F("FT Motion"));
SERIAL_ECHOPGM(" M494 T", (uint8_t)ftMotion.getTrajectoryType());
#if ENABLED(FTM_SMOOTHING)
SERIAL_ECHOPGM(
CARTES_PAIRED_LIST(
" X", c.smoothingTime.X, " Y", c.smoothingTime.Y,
" Z", c.smoothingTime.Z, " E", c.smoothingTime.E
)
);
#endif
if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6)
SERIAL_ECHOPGM(" O", c.poly6_acceleration_overshoot);
SERIAL_EOL();
}
/**
* M494: Set Fixed-time Motion Control Smoothing parameters
* M494: Set Fixed-time Motion Control parameters
*
* Parameters:
* T<type> Set trajectory generator type (0=TRAPEZOIDAL, 1=POLY5, 2=POLY6)
* O<overshoot> Set acceleration overshoot for POLY6 (1.25-1.875)
* X<time> Set smoothing time for the X axis
* Y<time> Set smoothing time for the Y axis
* Z<time> Set smoothing time for the Z axis
@ -67,59 +88,50 @@ void GcodeSuite::M494_report(const bool forReplay/*=true*/) {
void GcodeSuite::M494() {
bool report = !parser.seen_any();
#if HAS_X_AXIS
// Parse X axis smoothing time parameter.
if (parser.seenval('X')) {
const float val = parser.value_float();
if (WITHIN(val, 0.0f, FTM_MAX_SMOOTHING_TIME)) {
ftMotion.set_smoothing_time(X_AXIS, val);
report = true;
}
else
SERIAL_ECHOLNPGM("?Invalid ", C(STEPPER_A_NAME), " smoothing time [", C('X'), "] value.");
// Parse trajectory type parameter.
if (parser.seenval('T')) {
const int val = parser.value_int();
if (WITHIN(val, 0, 2)) {
planner.synchronize();
ftMotion.setTrajectoryType((TrajectoryType)val);
report = true;
}
#endif
else
SERIAL_ECHOLNPGM("?Invalid trajectory type [T] value. Use 0=TRAPEZOIDAL, 1=POLY5, 2=POLY6");
}
#if HAS_Y_AXIS
// Parse Y axis smoothing time parameter.
if (parser.seenval('Y')) {
const float val = parser.value_float();
if (WITHIN(val, 0.0f, FTM_MAX_SMOOTHING_TIME)) {
ftMotion.set_smoothing_time(Y_AXIS, val);
report = true;
}
else
SERIAL_ECHOLNPGM("?Invalid ", C(STEPPER_B_NAME), " smoothing time [", C('Y'), "] value.");
// Parse overshoot parameter.
if (parser.seenval('O')) {
const float val = parser.value_float();
if (WITHIN(val, 1.25f, 1.875f)) {
ftMotion.cfg.poly6_acceleration_overshoot = val;
report = true;
}
#endif
else
SERIAL_ECHOLNPGM("?Invalid overshoot [O] value. Range 1.25-1.875");
}
#if HAS_Z_AXIS
// Parse Z axis smoothing time parameter.
if (parser.seenval('Z')) {
const float val = parser.value_float();
if (WITHIN(val, 0.0f, FTM_MAX_SMOOTHING_TIME)) {
ftMotion.set_smoothing_time(Z_AXIS, val);
report = true;
#if ENABLED(FTM_SMOOTHING)
#define SMOOTH_SET(A,N) \
if (parser.seenval(CHARIFY(A))) { \
const float val = parser.value_float(); \
if (WITHIN(val, 0.0f, FTM_MAX_SMOOTHING_TIME)) { \
ftMotion.set_smoothing_time(_AXIS(A), val); \
report = true; \
} \
else \
SERIAL_ECHOLNPGM("?Invalid ", C(N), " smoothing time [", C(CHARIFY(A)), "] value."); \
}
else
SERIAL_ECHOLNPGM("?Invalid ", C(STEPPER_C_NAME), " smoothing time [", C('Z'), "] value.");
}
#endif
#if HAS_EXTRUDERS
// Parse E axis smoothing time parameter.
if (parser.seenval('E')) {
const float val = parser.value_float();
if (WITHIN(val, 0.0f, FTM_MAX_SMOOTHING_TIME)) {
ftMotion.set_smoothing_time(E_AXIS, val);
report = true;
}
else
SERIAL_ECHOLNPGM("?Invalid ", C('E'), " smoothing time [", C('E'), "] value.");
}
#endif
CARTES_GANG(
SMOOTH_SET(X, STEPPER_A_NAME), SMOOTH_SET(Y, STEPPER_B_NAME),
SMOOTH_SET(Z, STEPPER_C_NAME), SMOOTH_SET(E, 'E')
);
if (report) say_smoothing();
#endif // FTM_SMOOTHING
if (report) say_ftm_settings();
}
#endif // FTM_SMOOTHING
#endif // FT_MOTION

6
Marlin/src/gcode/gcode.h
View file

@ -1105,10 +1105,8 @@ private:
#if ENABLED(FT_MOTION)
static void M493();
static void M493_report(const bool forReplay=true);
#if ENABLED(FTM_SMOOTHING)
static void M494();
static void M494_report(const bool forReplay=true);
#endif
static void M494();
static void M494_report(const bool forReplay=true);
#endif
static void M500();

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

@ -489,6 +489,10 @@ namespace LanguageNarrow_en {
LSTR MSG_EN_STEPS = _UxGT("* Steps/mm");
LSTR MSG_TEMPERATURE = _UxGT("Temperature");
LSTR MSG_MOTION = _UxGT("Motion");
LSTR MSG_FTM_TRAPEZOIDAL = _UxGT("Trapezoidal");
LSTR MSG_FTM_POLY5 = _UxGT("5th Order");
LSTR MSG_FTM_POLY6 = _UxGT("6th Order");
LSTR MSG_FTM_TRAJECTORY = _UxGT("Trajectory: $");
LSTR MSG_FILAMENT = _UxGT("Filament");
LSTR MSG_FILAMENT_EN = _UxGT("Filament *");
LSTR MSG_VOLUMETRIC_ENABLED = _UxGT("E in mm") SUPERSCRIPT_THREE;
@ -945,6 +949,7 @@ namespace LanguageNarrow_en {
LSTR MSG_FTM_ZETA_N = _UxGT("@ Damping");
LSTR MSG_FTM_VTOL_N = _UxGT("@ Vib. Level");
LSTR MSG_FTM_SMOOTH_TIME_N = _UxGT("@ Smoothing Time");
LSTR MSG_FTM_POLY6_OVERSHOOT = _UxGT("@ Poly6 Overshoot");
LSTR MSG_LEVEL_X_AXIS = _UxGT("Level X Axis");
LSTR MSG_AUTO_CALIBRATE = _UxGT("Auto Calibrate");

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

@ -327,6 +327,15 @@ void menu_move() {
}
}
FSTR_P get_trajectory_name() {
switch (ftMotion.getTrajectoryType()) {
default:
case TrajectoryType::TRAPEZOIDAL: return GET_TEXT_F(MSG_FTM_TRAPEZOIDAL);
case TrajectoryType::POLY5: return GET_TEXT_F(MSG_FTM_POLY5);
case TrajectoryType::POLY6: return GET_TEXT_F(MSG_FTM_POLY6);
}
}
#if HAS_DYNAMIC_FREQ
FSTR_P get_dyn_freq_mode_name() {
switch (ftMotion.cfg.dynFreqMode) {
@ -363,6 +372,16 @@ void menu_move() {
SHAPED_MAP(MENU_FTM_SHAPER);
void menu_ftm_trajectory_generator() {
const TrajectoryType current_type = ftMotion.getTrajectoryType();
START_MENU();
BACK_ITEM(MSG_FIXED_TIME_MOTION);
if (current_type != TrajectoryType::TRAPEZOIDAL) ACTION_ITEM(MSG_FTM_TRAPEZOIDAL, []{ planner.synchronize(); ftMotion.setTrajectoryType(TrajectoryType::TRAPEZOIDAL); ui.go_back(); });
if (current_type != TrajectoryType::POLY5) ACTION_ITEM(MSG_FTM_POLY5, []{ planner.synchronize(); ftMotion.setTrajectoryType(TrajectoryType::POLY5); ui.go_back(); });
if (current_type != TrajectoryType::POLY6) ACTION_ITEM(MSG_FTM_POLY6, []{ planner.synchronize(); ftMotion.setTrajectoryType(TrajectoryType::POLY6); ui.go_back(); });
END_MENU();
}
#if HAS_DYNAMIC_FREQ
void menu_ftm_dyn_mode() {
@ -388,8 +407,8 @@ void menu_move() {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdangling-pointer"
#if ALL(__AVR__, HAS_MARLINUI_U8GLIB) && DISABLED(REDUCE_CODE_SIZE_FOR_FT_MOTION_ON_AVR)
#define CACHE_PREV_STRING
#if ALL(__AVR__, HAS_MARLINUI_U8GLIB) && DISABLED(OPTIMIZE_FT_MOTION_FOR_SIZE)
#define CACHE_FOR_SPEED 1
#endif
void menu_ft_motion() {
@ -401,10 +420,12 @@ void menu_move() {
// For U8G paged rendering check and skip extra string copy
#if HAS_X_AXIS
MString<20> shaper_name;
TERN_(CACHE_PREV_STRING, int8_t prev_a = -1);
#if CACHE_FOR_SPEED
int8_t prev_a = -1;
#endif
auto _shaper_name = [&](const AxisEnum a) {
if (TERN1(CACHE_PREV_STRING, a != prev_a)) {
TERN_(CACHE_PREV_STRING, prev_a = a);
if (TERN1(CACHE_FOR_SPEED, a != prev_a)) {
TERN_(CACHE_FOR_SPEED, prev_a = a);
shaper_name = get_shaper_name(a);
}
return shaper_name;
@ -412,18 +433,32 @@ void menu_move() {
#endif
#if HAS_DYNAMIC_FREQ
MString<20> dmode;
TERN_(CACHE_PREV_STRING, bool got_d = false);
#if CACHE_FOR_SPEED
bool got_d = false;
#endif
auto _dmode = [&]{
if (TERN1(CACHE_PREV_STRING, !got_d)) {
TERN_(CACHE_PREV_STRING, got_d = true);
if (TERN1(CACHE_FOR_SPEED, !got_d)) {
TERN_(CACHE_FOR_SPEED, got_d = true);
dmode = get_dyn_freq_mode_name();
}
return dmode;
};
#endif
MString<20> traj_name;
#if CACHE_FOR_SPEED
bool got_t = false;
#endif
auto _traj_name = [&]{
if (TERN1(CACHE_FOR_SPEED, !got_t)) {
TERN_(CACHE_FOR_SPEED, got_t = true);
traj_name = get_trajectory_name();
}
return traj_name;
};
#else
auto _shaper_name = [](const AxisEnum a) { return get_shaper_name(a); };
auto _dmode = []{ return get_dyn_freq_mode_name(); };
auto _traj_name = []{ return get_trajectory_name(); };
#endif
START_MENU();
@ -445,6 +480,11 @@ void menu_move() {
if (AXIS_IS_EISHAPING(A)) \
EDIT_ITEM_FAST_N(float42_52, _AXIS(A), MSG_FTM_VTOL_N, &c.vtol.A, 0.0f, 1.0f, ftMotion.update_shaping_params); \
}
SUBMENU_S(_traj_name(), MSG_FTM_TRAJECTORY, menu_ftm_trajectory_generator);
if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6)
EDIT_ITEM(float42_52, MSG_FTM_POLY6_OVERSHOOT, &c.poly6_acceleration_overshoot, 1.25f, 1.875f);
SHAPED_MAP(SHAPER_MENU_ITEM);
#if HAS_DYNAMIC_FREQ
@ -486,15 +526,18 @@ void menu_move() {
void menu_tune_ft_motion() {
// Define stuff ahead of the menu loop
ft_config_t &c = ftMotion.cfg;
#ifdef __AVR__
// Copy Flash strings to RAM for C-string substitution
// For U8G paged rendering check and skip extra string copy
#if HAS_X_AXIS
MString<20> shaper_name;
TERN_(CACHE_PREV_STRING, int8_t prev_a = -1);
#if CACHE_FOR_SPEED
int8_t prev_a = -1;
#endif
auto _shaper_name = [&](const AxisEnum a) {
if (TERN1(CACHE_PREV_STRING, a != prev_a)) {
TERN_(CACHE_PREV_STRING, prev_a = a);
if (TERN1(CACHE_FOR_SPEED, a != prev_a)) {
TERN_(CACHE_FOR_SPEED, prev_a = a);
shaper_name = get_shaper_name(a);
}
return shaper_name;
@ -502,38 +545,63 @@ void menu_move() {
#endif
#if HAS_DYNAMIC_FREQ
MString<20> dmode;
TERN_(CACHE_PREV_STRING, bool got_d = false);
#if CACHE_FOR_SPEED
bool got_d = false;
#endif
auto _dmode = [&]{
if (TERN1(CACHE_PREV_STRING, !got_d)) {
TERN_(CACHE_PREV_STRING, got_d = true);
if (TERN1(CACHE_FOR_SPEED, !got_d)) {
TERN_(CACHE_FOR_SPEED, got_d = true);
dmode = get_dyn_freq_mode_name();
}
return dmode;
};
#endif
#else
MString<20> traj_name;
#if CACHE_FOR_SPEED
bool got_t = false;
#endif
auto _traj_name = [&]{
if (TERN1(CACHE_FOR_SPEED, !got_t)) {
TERN_(CACHE_FOR_SPEED, got_t = true);
traj_name = get_trajectory_name();
}
return traj_name;
};
#else // !__AVR__
auto _shaper_name = [](const AxisEnum a) { return get_shaper_name(a); };
auto _dmode = []{ return get_dyn_freq_mode_name(); };
#endif
#if HAS_EXTRUDERS
ft_config_t &c = ftMotion.cfg;
auto _traj_name = []{ return get_trajectory_name(); };
#endif
START_MENU();
BACK_ITEM(MSG_TUNE);
SUBMENU_S(_traj_name(), MSG_FTM_TRAJECTORY, menu_ftm_trajectory_generator);
if (ftMotion.getTrajectoryType() == TrajectoryType::POLY6)
EDIT_ITEM(float42_52, MSG_FTM_POLY6_OVERSHOOT, &c.poly6_acceleration_overshoot, 1.25f, 1.875f);
#define _CMPM_MENU_ITEM(A) SUBMENU_N_S(_AXIS(A), _shaper_name(_AXIS(A)), MSG_FTM_CMPN_MODE, menu_ftm_shaper_##A);
SHAPED_MAP(_CMPM_MENU_ITEM);
#if HAS_DYNAMIC_FREQ
SUBMENU_S(_dmode(), MSG_FTM_DYN_MODE, menu_ftm_dyn_mode);
#endif
#if HAS_EXTRUDERS
EDIT_ITEM(bool, MSG_LINEAR_ADVANCE, &c.linearAdvEna);
if (c.linearAdvEna || ENABLED(FT_MOTION_NO_MENU_TOGGLE))
EDIT_ITEM(float42_52, MSG_ADVANCE_K, &c.linearAdvK, 0.0f, 10.0f);
#endif
#if ENABLED(FTM_SMOOTHING)
#define _SMOO_MENU_ITEM(A) do{ \
editable.decimal = c.smoothingTime.A; \
EDIT_ITEM_FAST_N(float43, _AXIS(A), MSG_FTM_SMOOTH_TIME_N, &editable.decimal, 0.0f, FTM_MAX_SMOOTHING_TIME, []{ ftMotion.set_smoothing_time(_AXIS(A), editable.decimal); }); \
}while(0);
CARTES_MAP(_SMOO_MENU_ITEM);
#endif
END_MENU();
} // menu_tune_ft_motion

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

@ -25,6 +25,9 @@
#if ENABLED(FT_MOTION)
#include "ft_motion.h"
#include "ft_motion/trapezoidal_trajectory_generator.h"
#include "ft_motion/poly5_trajectory_generator.h"
#include "ft_motion/poly6_trajectory_generator.h"
#include "stepper.h" // Access stepper block queue function and abort status.
#include "endstops.h"
@ -62,22 +65,18 @@ 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.
// Trapezoid data variables.
// Block data variables.
xyze_pos_t FTMotion::startPos, // (mm) Start position of block
FTMotion::endPos_prevBlock = { 0.0f }; // (mm) End position of previous block
xyze_float_t FTMotion::ratio; // (ratio) Axis move ratio of block
float FTMotion::accel_P, // Acceleration prime of block. [mm/sec/sec]
FTMotion::decel_P, // Deceleration prime of block. [mm/sec/sec]
FTMotion::F_P, // Feedrate prime of block. [mm/sec]
FTMotion::f_s, // Starting feedrate of block. [mm/sec]
FTMotion::s_1e, // Position after acceleration phase of block.
FTMotion::s_2e; // Position after acceleration and coasting phase of block.
float FTMotion::tau = 0.0f; // (s) Time since start of block
uint32_t FTMotion::N1, // Number of data points in the acceleration phase.
FTMotion::N2, // Number of data points in the coasting phase.
FTMotion::N3; // Number of data points in the deceleration phase.
uint32_t FTMotion::max_intervals; // Total number of data points that will be generated from block.
// Trajectory generators
TrapezoidalTrajectoryGenerator FTMotion::trapezoidalGenerator;
Poly5TrajectoryGenerator FTMotion::poly5Generator;
Poly6TrajectoryGenerator FTMotion::poly6Generator;
TrajectoryGenerator& FTMotion::currentGenerator = FTMotion::trapezoidalGenerator;
TrajectoryType FTMotion::trajectoryType = TrajectoryType::FTM_TRAJECTORY_TYPE;
// Make vector variables.
uint32_t FTMotion::traj_idx_get = 0, // Index of fixed time trajectory generation of the overall block.
@ -439,7 +438,7 @@ void FTMotion::reset() {
blockProcRdy = batchRdy = batchRdyForInterp = false;
endPos_prevBlock.reset();
tau = 0;
traj_idx_get = 0;
traj_idx_set = TERN(FTM_UNIFIED_BWS, 0, _MIN(BATCH_SIDX_IN_WINDOW, FTM_BATCH_SIZE));
@ -484,7 +483,6 @@ void FTMotion::discard_planner_block_protected() {
void FTMotion::runoutBlock() {
startPos = endPos_prevBlock;
ratio.reset();
const int32_t n_to_fill_batch = (FTM_WINDOW_SIZE) - traj_idx_set;
@ -492,11 +490,17 @@ void FTMotion::runoutBlock() {
const int32_t n_to_settle_shaper = num_samples_shaper_settle();
const int32_t n_diff = n_to_settle_shaper - n_to_fill_batch,
n_to_fill_batch_after_settling = n_diff > 0 ? (FTM_BATCH_SIZE) - (n_diff % (FTM_BATCH_SIZE)) : -n_diff;
n_to_fill_batch_after_settling = n_diff > 0 ? (FTM_BATCH_SIZE) - (n_diff % (FTM_BATCH_SIZE)) : -n_diff;
max_intervals = PROP_BATCHES * (FTM_BATCH_SIZE) + n_to_settle_shaper + n_to_fill_batch_after_settling;
ratio.reset();
uint32_t max_intervals = PROP_BATCHES * (FTM_BATCH_SIZE) + n_to_settle_shaper + n_to_fill_batch_after_settling;
const float reminder_from_last_block = - tau;
const float total_duration = max_intervals * FTM_TS + reminder_from_last_block;
blockProcRdy = true;
// Plan a zero-motion trajectory for runout
currentGenerator.planRunout(total_duration);
blockProcRdy = true; // since ratio is 0, the trajectory positions won't advance in any axis
}
// Auxiliary function to get number of step commands in the buffer.
@ -509,9 +513,22 @@ int32_t FTMotion::stepperCmdBuffItems() {
void FTMotion::init() {
update_shaping_params();
TERN_(FTM_SMOOTHING, update_smoothing_params());
setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE);
reset(); // Precautionary.
}
// Set trajectory generator type
void FTMotion::setTrajectoryType(const TrajectoryType type) {
cfg.trajectory_type = trajectoryType = type;
switch (type) {
default: cfg.trajectory_type = trajectoryType = TrajectoryType::FTM_TRAJECTORY_TYPE;
case TrajectoryType::TRAPEZOIDAL: currentGenerator = trapezoidalGenerator; break;
case TrajectoryType::POLY5: currentGenerator = poly5Generator; break;
case TrajectoryType::POLY6: currentGenerator = poly6Generator; break;
}
currentGenerator.reset(); // Reset the selected generator
}
// Load / convert block data from planner to fixed-time control variables.
void FTMotion::loadBlockData(block_t * const current_block) {
// Cache the extruder index for this block
@ -525,89 +542,24 @@ void FTMotion::loadBlockData(block_t * const current_block) {
ratio = moveDist * oneOverLength;
const float mmps = totalLength / current_block->step_event_count; // (mm/step) Distance for each step
const float initial_speed = mmps * current_block->initial_rate; // (mm/s) Start feedrate
const float final_speed = mmps * current_block->final_rate; // (mm/s) End feedrate
const float accel = current_block->acceleration;
const float nominal_speed = current_block->nominal_speed;
f_s = mmps * current_block->initial_rate; // (steps/s) Start feedrate
// Plan the trajectory using the trajectory generator
currentGenerator.plan(initial_speed, final_speed, accel, nominal_speed, totalLength);
const float f_e = mmps * current_block->final_rate; // (steps/s) End feedrate
/* Keep for comprehension
const float a = current_block->acceleration, // (mm/s^2) Same magnitude for acceleration or deceleration
oneby2a = 1.0f / (2.0f * a), // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2
oneby2d = -oneby2a; // (s/mm) Time to accelerate or decelerate one mm (i.e., oneby2a * 2
const float fsSqByTwoA = sq(f_s) * oneby2a, // (mm) Distance to accelerate from start speed to nominal speed
feSqByTwoD = sq(f_e) * oneby2d; // (mm) Distance to decelerate from nominal speed to end speed
float F_n = current_block->nominal_speed; // (mm/s) Speed we hope to achieve, if possible
const float fdiff = feSqByTwoD - fsSqByTwoA, // (mm) Coasting distance if nominal speed is reached
odiff = oneby2a - oneby2d, // (i.e., oneby2a * 2) (mm/s) Change in speed for one second of acceleration
ldiff = totalLength - fdiff; // (mm) Distance to travel if nominal speed is reached
float T2 = (1.0f / F_n) * (ldiff - odiff * sq(F_n)); // (s) Coasting duration after nominal speed reached
if (T2 < 0.0f) {
T2 = 0.0f;
F_n = SQRT(ldiff / odiff); // Clip by intersection if nominal speed can't be reached.
}
const float T1 = (F_n - f_s) / a, // (s) Accel Time = difference in feedrate over acceleration
T3 = (F_n - f_e) / a; // (s) Decel Time = difference in feedrate over acceleration
*/
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
N3 = CEIL(T3 * (FTM_FS)); // Decel
const float T1_P = N1 * (FTM_TS), // (s) Accel datapoints x timestep resolution
T2_P = N2 * (FTM_TS), // (s) Coast
T3_P = N3 * (FTM_TS); // (s) Decel
/**
* Calculate the reachable feedrate at the end of the accel phase.
* totalLength is the total distance to travel in mm
* f_s : (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
*/
const float adist = f_s * T1_P;
F_P = (2.0f * totalLength - adist - f_e * T3_P) / (T1_P + 2.0f * T2_P + T3_P); // (mm/s) Feedrate at the end of the accel phase
// Calculate the acceleration and deceleration rates
accel_P = N1 ? ((F_P - f_s) / T1_P) : 0.0f;
decel_P = (f_e - F_P) / T3_P;
// Calculate the distance traveled during the accel phase
s_1e = adist + 0.5f * accel_P * sq(T1_P);
// Calculate the distance traveled during the decel phase
s_2e = s_1e + F_P * T2_P;
// Accel + Coasting + Decel datapoints
max_intervals = N1 + N2 + N3;
// Accel + Coasting + Decel + datapoints
const float reminder_from_last_block = - tau;
endPos_prevBlock += moveDist;
TERN_(FTM_HAS_LIN_ADVANCE, use_advance_lead = current_block->use_advance_lead);
// Watch endstops until the move ends
const float total_duration = currentGenerator.getTotalDuration();
uint32_t max_intervals = ceil((total_duration + reminder_from_last_block) * FTM_FS);
const millis_t move_end_ti = millis() + SEC_TO_MS((FTM_TS) * float(max_intervals + num_samples_shaper_settle() + ((PROP_BATCHES) + 1) * (FTM_BATCH_SIZE)) + (float(FTM_STEPPERCMD_BUFF_SIZE) / float(FTM_STEPPER_FS)));
#define _SET_MOVE_END(A) do{ \
@ -622,24 +574,24 @@ void FTMotion::loadBlockData(block_t * const current_block) {
// Generate data points of the trajectory.
void FTMotion::generateTrajectoryPointsFromBlock() {
const float total_duration = currentGenerator.getTotalDuration();
if (tau + FTM_TS > total_duration) {
// TODO: refactor code so this thing is not twice.
// the reason of it being in the beginning, is that a block can be so short that it has
// zero trajectories.
// the next iteration will fall beyond this block
blockProcRdy = false;
traj_idx_get = 0;
tau -= total_duration;
return;
}
do {
float tau = (traj_idx_get + 1) * (FTM_TS); // (s) Time since start of block
float dist = 0.0f; // (mm) Distance traveled
tau += FTM_TS; // (s) Time since start of block
// If the end of the last block doesn't exactly land on a trajectory index,
// tau can start negative, but it always holds that `tau > -FTM_TS`
if (traj_idx_get < N1) {
// Acceleration phase
dist = (f_s * tau) + (0.5f * accel_P * sq(tau)); // (mm) Distance traveled for acceleration phase since start of block
}
else if (traj_idx_get < (N1 + N2)) {
// Coasting phase
dist = s_1e + F_P * (tau - N1 * (FTM_TS)); // (mm) Distance traveled for coasting phase since start of block
//TERN_(HAS_EXTRUDERS, accel_k = 0.0f);
}
else {
// 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
}
// Get distance from trajectory generator
const float dist = currentGenerator.getDistanceAtTime(tau);
#define _SET_TRAJ(q) traj.q[traj_idx_set] = startPos.q + ratio.q * dist;
LOGICAL_AXIS_MAP_LC(_SET_TRAJ);
@ -753,9 +705,12 @@ void FTMotion::generateTrajectoryPointsFromBlock() {
traj_idx_set = BATCH_SIDX_IN_WINDOW;
batchRdy = true;
}
if (++traj_idx_get == max_intervals) {
traj_idx_get++;
if (tau + FTM_TS > total_duration) {
// the next iteration will fall beyond this block
blockProcRdy = false;
traj_idx_get = 0;
tau -= total_duration;
}
} while (blockProcRdy && !batchRdy);
} // generateTrajectoryPointsFromBlock

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

@ -26,6 +26,10 @@
#include "stepper.h" // For stepper motion and direction
#include "ft_types.h"
#include "ft_motion/trajectory_generator.h"
#include "ft_motion/trapezoidal_trajectory_generator.h"
#include "ft_motion/poly5_trajectory_generator.h"
#include "ft_motion/poly6_trajectory_generator.h"
#if HAS_X_AXIS && (HAS_Z_AXIS || HAS_EXTRUDERS)
#define HAS_DYNAMIC_FREQ 1
@ -69,6 +73,8 @@ typedef struct FTConfig {
float linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; // Linear advance gain.
#endif
TrajectoryType trajectory_type = TrajectoryType::TRAPEZOIDAL; // Trajectory generator type
float poly6_acceleration_overshoot; // Overshoot factor for Poly6 (1.25 to 2.0)
} ft_config_t;
class FTMotion {
@ -117,6 +123,10 @@ class FTMotion {
cfg.linearAdvK = FTM_LINEAR_ADV_DEFAULT_K;
#endif
cfg.poly6_acceleration_overshoot = FTM_POLY6_ACCELERATION_OVERSHOOT;
setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE);
reset();
}
@ -147,6 +157,10 @@ class FTMotion {
static void reset(); // Reset all states of the fixed time conversion to defaults.
// Trajectory generator selection
static void setTrajectoryType(const TrajectoryType type);
static TrajectoryType getTrajectoryType() { return trajectoryType; }
FORCE_INLINE static bool axis_is_moving(const AxisEnum axis) {
return cfg.active ? PENDING(millis(), axis_move_end_ti[axis]) : stepper.axis_is_moving(axis);
}
@ -162,18 +176,18 @@ class FTMotion {
static bool blockProcRdy;
static bool batchRdy, batchRdyForInterp;
// Trapezoid data variables.
// Block data variables.
static xyze_pos_t startPos, // (mm) Start position of block
endPos_prevBlock; // (mm) End position of previous block
static xyze_float_t ratio; // (ratio) Axis move ratio of block
static float accel_P, decel_P,
F_P,
f_s,
s_1e,
s_2e;
static float tau; // (s) Time since start of block
static uint32_t N1, N2, N3;
static uint32_t max_intervals;
// Trajectory generators
static TrapezoidalTrajectoryGenerator trapezoidalGenerator;
static Poly5TrajectoryGenerator poly5Generator;
static Poly6TrajectoryGenerator poly6Generator;
static TrajectoryGenerator& currentGenerator;
static TrajectoryType trajectoryType;
// Number of batches needed to propagate the current trajectory to the stepper.
static constexpr uint32_t PROP_BATCHES = CEIL((FTM_WINDOW_SIZE) / (FTM_BATCH_SIZE)) - 1;

122
Marlin/src/module/ft_motion/poly5_trajectory_generator.h Normal file
View file

@ -0,0 +1,122 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "trajectory_generator.h"
#include <math.h>
/**
* 5th order polynomial trajectory generator.
* Provides continuous jerk, resulting in smooth acceleration.
* Acceleration starts and ends at zero. The jerk, snap, and crackle are such that
* the distance and phase durations match those of a trapezoidal profile.
*/
class Poly5TrajectoryGenerator : public TrajectoryGenerator {
public:
Poly5TrajectoryGenerator() = default;
void plan(float initial_speed, float final_speed, float acceration, float nominal_speed, float distance) override {
this->initial_speed = initial_speed;
this->nominal_speed = nominal_speed;
// Calculate timing phases using the same logic as trapezoidal generator
const float one_over_acc = 1.0f / acceration;
const float ldiff = distance + 0.5f * one_over_acc * (sq(this->initial_speed) + sq(final_speed));
T2 = ldiff / this->nominal_speed - one_over_acc * this->nominal_speed;
if (T2 < 0.0f) {
T2 = 0.0f;
this->nominal_speed = sqrtf(ldiff * acceration);
}
T1 = (this->nominal_speed - this->initial_speed) * one_over_acc;
T3 = (this->nominal_speed - final_speed) * one_over_acc;
const float d1 = (this->initial_speed + this->nominal_speed) * T1 * 0.5f;
const float T1_2 = T1 * T1;
const float T1_3 = T1_2 * T1;
const float T1_4 = T1_3 * T1;
const float T1_5 = T1_4 * T1;
// acc_c0 = 0.0f; // initial position is zero
acc_c1 = this->initial_speed;
// acc_c2 = 0.0f; // initial acceleration is zero
acc_c3 = (10.0f * d1 - (6.0f * this->initial_speed + 4.0f * this->nominal_speed) * T1) / T1_3;
acc_c4 = (15.0f * d1 - (8.0f * this->initial_speed + 7.0f * this->nominal_speed) * T1) / -T1_4;
acc_c5 = (6.0f * d1 - 3.0f * (this->initial_speed + this->nominal_speed) * T1) / T1_5;
pos_before_coast = d1;
// Coast phase
pos_after_coast = pos_before_coast + this->nominal_speed * T2;
// Deceration phase
const float d3 = (this->nominal_speed + final_speed) * T3 * 0.5f;
const float T3_2 = T3 * T3;
const float T3_3 = T3_2 * T3;
const float T3_4 = T3_3 * T3;
const float T3_5 = T3_4 * T3;
// dec_c0 = 0.0f; // initial position is zero
dec_c1 = this->nominal_speed;
// dec_c2 = 0.0f; // initial acceleration is zero
dec_c3 = (10.0f * d3 - (6.0f * this->nominal_speed + 4.0f * final_speed) * T3) / T3_3;
dec_c4 = (15.0f * d3 - (8.0f * this->nominal_speed + 7.0f * final_speed) * T3) / -T3_4;
dec_c5 = (6.0f * d3 - 3.0f * (this->nominal_speed + final_speed) * T3) / T3_5;
}
void planRunout(float duration) override {
reset();
T2 = duration;
}
float getDistanceAtTime(float t) const override {
if (t < T1) {
// Acceration phase
return t * (acc_c1 + t * t * (acc_c3 + t * (acc_c4 + t * acc_c5)));
} else if (t <= (T1 + T2)) {
// Coasting phase
return pos_before_coast + this->nominal_speed * (t - T1);
}
// Deceration phase
const float tau = t - (T1 + T2);
return pos_after_coast + tau * (dec_c1 + tau * tau * (dec_c3 + tau * (dec_c4 + tau * dec_c5)));
}
float getTotalDuration() const override { return T1 + T2 + T3; }
void reset() override {
acc_c1 = acc_c3 = acc_c4 = acc_c5 = 0.0f;
dec_c1 = dec_c3 = dec_c4 = dec_c5 = 0.0f;
T1 = T2 = T3 = 0.0f;
initial_speed = nominal_speed = 0.0f;
pos_before_coast = pos_after_coast = 0.0f;
}
private:
// c1: initial velocity, c3: jerk, c4: snap, c5: crackle
// acceleration coefficients
float acc_c1 = 0.0f, acc_c3 = 0.0f, acc_c4 = 0.0f, acc_c5 = 0.0f;
// deceleration coefficients
float dec_c1 = 0.0f, dec_c3 = 0.0f, dec_c4 = 0.0f, dec_c5 = 0.0f;
// timestamps of each phase
float T1 = 0.0f, T2 = 0.0f, T3 = 0.0f;
float initial_speed = 0.0f, nominal_speed = 0.0f;
float pos_before_coast = 0.0f, pos_after_coast = 0.0f;
};

142
Marlin/src/module/ft_motion/poly6_trajectory_generator.cpp Normal file
View file

@ -0,0 +1,142 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#include "../../inc/MarlinConfig.h"
#if ENABLED(FT_MOTION)
#include "poly6_trajectory_generator.h"
#include <math.h>
#include "../ft_motion.h"
Poly6TrajectoryGenerator::Poly6TrajectoryGenerator() {}
void Poly6TrajectoryGenerator::plan(const float initial_speed, const float final_speed, const float acceleration, float nominal_speed, const float distance) {
this->initial_speed = initial_speed;
this->nominal_speed = nominal_speed;
// --- Trapezoid timings (unchanged) ---
const float invA = 1.0f / acceleration;
const float ldiff = distance + 0.5f * invA * (sq(this->initial_speed) + sq(final_speed));
T2 = ldiff / this->nominal_speed - invA * this->nominal_speed;
if (T2 < 0.0f) {
T2 = 0.0f;
this->nominal_speed = sqrtf(ldiff * acceleration);
}
T1 = (this->nominal_speed - this->initial_speed) * invA;
T3 = (this->nominal_speed - final_speed) * invA;
// Distances at phase boundaries (trapezoid areas)
pos_before_coast = 0.5f * (this->initial_speed + this->nominal_speed) * T1;
pos_after_coast = pos_before_coast + this->nominal_speed * T2;
// --- Build sextic (in position) for each phase ---
// We start from a quintic-in-position s5(u) that meets endpoints with a(0)=a(1)=0,
// then add c*K(u) where K(u) = u^3(1-u)^3 to hit a_mid target exactly.
// Parameterization: u=t/Tphase in [0,1].
// Common mid values for K''(u)
constexpr float Kpp_mid = 6 * 0.5f - 36 * 0.25f + 60 * 0.125f - 30 * 0.0625f; // = 1.875f
// ---- Accel phase ----
{
const float Ts = T1;
const float s0 = 0.0f;
const float v0 = this->initial_speed;
const float s1 = pos_before_coast;
const float v1 = this->nominal_speed;
const float delta_p = s1 - s0 - v0 * Ts;
const float delta_v = (v1 - v0) * Ts;
// s5(u) = s0 + v0*Ts*u + c3 u^3 + c4 u^4 + c5 u^5
acc_c3 = 10.0f * delta_p - 4.0f * delta_v;
acc_c4 = -15.0f * delta_p + 7.0f * delta_v;
acc_c5 = 6.0f * delta_p - 3.0f * delta_v;
// a5_mid = s5''(0.5)/Ts^2
const float a5_mid = s5pp_u(acc_c3, acc_c4, acc_c5, 0.5f) / (Ts*Ts);
const float a_mid_target = ftMotion.cfg.poly6_acceleration_overshoot * acceleration;
// c chosen so that (s5''(0.5)+c5*K''(0.5))/Ts^2 == a_mid_target, meaning at half the acc/decc phase
acc_c6 = (Ts*Ts) * (a_mid_target - a5_mid) / Kpp_mid;
}
// ---- Decel phase ----
{
const float Ts = T3;
const float s0 = pos_after_coast;
const float v0 = this->nominal_speed;
const float s1 = pos_after_coast + 0.5f * (this->nominal_speed + final_speed) * T3;
const float v1 = final_speed;
const float delta_p = s1 - s0 - v0 * Ts;
const float delta_v = (v1 - v0) * Ts;
dec_c3 = 10.0f * delta_p - 4.0f * delta_v;
dec_c4 = -15.0f * delta_p + 7.0f * delta_v;
dec_c5 = 6.0f * delta_p - 3.0f * delta_v;
const float a5_mid = s5pp_u(dec_c3, dec_c4, dec_c5, 0.5f) / (Ts * Ts);
const float a_mid_target = -ftMotion.cfg.poly6_acceleration_overshoot * acceleration;
dec_c6 = (Ts * Ts) * (a_mid_target - a5_mid) / Kpp_mid;
}
}
void Poly6TrajectoryGenerator::planRunout(const float duration) {
reset();
T2 = duration;
}
float Poly6TrajectoryGenerator::getDistanceAtTime(const float t) const {
if (t < T1) {
// Accel phase: u=t/T1
const float u = t / T1;
return s5_u(0.0f, initial_speed, T1, acc_c3, acc_c4, acc_c5, u)
+ acc_c6 * K_u(0.0f, initial_speed, T1, u); // K added as pure shape (position domain)
}
else if (t <= (T1 + T2)) {
// Coast
return pos_before_coast + this->nominal_speed * (t - T1);
}
// Decel phase
const float tau = t - (T1 + T2);
const float u = tau / T3;
return s5_u(pos_after_coast, this->nominal_speed, T3, dec_c3, dec_c4, dec_c5, u)
+ dec_c6 * K_u(pos_after_coast, this->nominal_speed, T3, u);
}
float Poly6TrajectoryGenerator::getTotalDuration() const { return T1 + T2 + T3; }
void Poly6TrajectoryGenerator::reset() {
T1 = T2 = T3 = 0.0f;
initial_speed = nominal_speed = 0.0f;
pos_before_coast = pos_after_coast = 0.0f;
acc_c3 = acc_c4 = acc_c5 = 0.0f;
dec_c3 = dec_c4 = dec_c5 = 0.0f;
acc_c6 = dec_c6 = 0.0f;
}
#endif // FT_MOTION

75
Marlin/src/module/ft_motion/poly6_trajectory_generator.h Normal file
View file

@ -0,0 +1,75 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "trajectory_generator.h"
#include <math.h>
/**
* 6th order S-Curve trajectory generator.
* Applies a sextic (position) only to accel/decel phases, coast is flat.
* - T1/T2/T3 exactly match trapezoid timings
* - a(mid-phase) = overshoot * a_max (accel) and = -overshoot * a_max (decel)
* - v,a start/end at 0 within each phase; joins are continuous.
*/
class Poly6TrajectoryGenerator : public TrajectoryGenerator {
public:
Poly6TrajectoryGenerator();
void plan(const float initial_speed, const float final_speed, const float acceleration, float nominal_speed, const float distance) override;
void planRunout(const float duration) override;
float getDistanceAtTime(const float t) const override;
float getTotalDuration() const override;
void reset() override;
private:
// ===== Utilities (position domain) =====
// Base quintic in position (end accel = 0): s5(u) = s0 + v0*Ts*u + c3 u^3 + c4 u^4 + c5 u^5
static inline float s5_u(const float s0, const float v0, const float Ts,
const float c3, const float c4, const float c5, const float u) {
const float u2=u * u, u3=u2 * u, u4=u3 * u, u5=u4 * u;
return s0 + v0 * Ts * u + c3 * u3 + c4 * u4 + c5 * u5;
}
static inline float s5pp_u(const float c3, const float c4, const float c5, const float u) {
// d^2/du^2 (c3 u^3 + c4 u^4 + c5 u^5) = 6a u + 12 c4 u^2 + 20 c5 u^3
return 6.0f * c3 * u + 12.0f * c4 * u * u + 20.0f * c5 * u * u * u;
}
// Shape term K(u) = u^3(1-u)^3 added in position with scalar c
static inline float K_u(const float /*s0*/, const float /*v0*/, const float /*Ts*/, const float u) {
const float um1 = 1.0f - u;
return (u * u * u) * (um1 * um1 * um1);
}
// Timings and kinematics
float T1 = 0.0f, T2 = 0.0f, T3 = 0.0f;
float initial_speed = 0.0f, nominal_speed = 0.0f;
float pos_before_coast = 0.0f, pos_after_coast = 0.0f;
// New phase polynomials
float acc_c3 = 0.0f, acc_c4 = 0.0f, acc_c5 = 0.0f, acc_c6 = 0.0f;
float dec_c3 = 0.0f, dec_c4 = 0.0f, dec_c5 = 0.0f, dec_c6 = 0.0f;
};

77
Marlin/src/module/ft_motion/trajectory_generator.h Normal file
View file

@ -0,0 +1,77 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "../../inc/MarlinConfig.h"
#include <stdint.h>
/**
* Base class for trajectory generators.
* Provides a zero-cost abstraction for different trajectory generation algorithms.
*/
class TrajectoryGenerator {
public:
/**
* Plan a trajectory with the given parameters.
* @param initial_speed Starting feedrate [mm/s]
* @param final_speed Ending feedrate [mm/s]
* @param acceleration Acceleration [mm/s²]
* @param nominal_speed Peak feedrate [mm/s]
* @param distance Total distance to travel [mm]
*/
virtual void plan(float initial_speed, float final_speed, float acceleration, float nominal_speed, float distance) = 0;
/**
* Plan a zero-motion trajectory for a specific duration.
* @param duration The total time for the runout phase [s]
*/
virtual void planRunout(float duration) = 0;
/**
* Get the distance traveled at time t.
* @param t Time since start of trajectory [s]
* @return Distance traveled [mm]
*/
virtual float getDistanceAtTime(float t) const = 0;
/**
* Get the total duration of the trajectory.
* @return Total time [s]
*/
virtual float getTotalDuration() const = 0;
/**
* Reset the trajectory generator to initial state.
*/
virtual void reset() = 0;
protected:
// Protected constructor to prevent direct instantiation
TrajectoryGenerator() = default;
virtual ~TrajectoryGenerator() = default;
};
/**
* Trajectory generator types for runtime selection
*/
enum class TrajectoryType : uint8_t { TRAPEZOIDAL, POLY5, POLY6 };

98
Marlin/src/module/ft_motion/trapezoidal_trajectory_generator.h Normal file
View file

@ -0,0 +1,98 @@
/**
* Marlin 3D Printer Firmware
* Copyright (c) 2025 MarlinFirmware [https://github.com/MarlinFirmware/Marlin]
*
* Based on Sprinter and grbl.
* Copyright (c) 2011 Camiel Gubbels / Erik van der Zalm
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <https://www.gnu.org/licenses/>.
*
*/
#pragma once
#include "trajectory_generator.h"
#include <math.h>
/**
* Trapezoidal trajectory generator.
* Provides continuous velocity, but acceleration is discontinuous.
* Implements a trapezoidal velocity profile with acceleration, cruise, and deceleration phases.
*/
class TrapezoidalTrajectoryGenerator : public TrajectoryGenerator {
public:
TrapezoidalTrajectoryGenerator() = default;
void plan(float initial_speed, float final_speed, float acceleration, float nominal_speed, float distance) override {
this->initial_speed = initial_speed;
this->nominal_speed = nominal_speed;
this->acceleration = acceleration;
const float one_over_accel = 1.0f / acceleration;
const float ldiff = distance + 0.5f * one_over_accel * (initial_speed * initial_speed + final_speed * final_speed);
T2 = ldiff / nominal_speed - one_over_accel * nominal_speed;
if (T2 < 0.0f) {
T2 = 0.0f;
this->nominal_speed = sqrtf(ldiff * acceleration);
}
T1 = (this->nominal_speed - initial_speed) * one_over_accel;
T3 = (this->nominal_speed - final_speed) * one_over_accel;
// Calculate the distance traveled during the accel phase
pos_before_coast = initial_speed * T1 + 0.5f * acceleration * T1 * T1;
// Calculate the distance traveled during the coast phase
pos_after_coast = pos_before_coast + this->nominal_speed * T2;
}
float getDistanceAtTime(float t) const override {
if (t < T1) {
// Acceleration phase
return (this->initial_speed * t) + (0.5f * this->acceleration * t * t);
} else if (t <= (T1 + T2)) {
// Coasting phase
return pos_before_coast + this->nominal_speed * (t - T1);
}
// Deceleration phase
const float tau_decel = t - (T1 + T2);
return pos_after_coast + this->nominal_speed * tau_decel - 0.5f * this->acceleration * tau_decel * tau_decel;
}
float getTotalDuration() const override {
return T1 + T2 + T3;
}
void planRunout(float duration) override {
reset();
T2 = duration; // Coast at zero speed for the entire duration
}
void reset() override {
T1 = T2 = T3 = 0.0f;
this->initial_speed = this->nominal_speed = this->acceleration = 0.0f;
pos_before_coast = pos_after_coast = 0.0f;
}
private:
// Internal trajectory parameters - kept private
float T1 = 0.0f; // Duration of acceleration phase [s]
float T2 = 0.0f; // Duration of coasting phase [s]
float T3 = 0.0f; // Duration of deceleration phase [s]
float initial_speed = 0.0f; // Starting feedrate [mm/s]
float nominal_speed = 0.0f; // Peak feedrate [mm/s]
float acceleration = 0.0f; // Acceleration [mm/s²]
float pos_before_coast = 0.0f; // Position after acceleration phase [mm]
float pos_after_coast = 0.0f; // Position after acceleration and coasting phase [mm]
};

3
buildroot/share/PlatformIO/scripts/common-cxxflags.py
View file

@ -11,7 +11,8 @@ if pioutil.is_pio_build():
# "-Wno-incompatible-pointer-types",
# "-Wno-unused-const-variable",
# "-Wno-maybe-uninitialized",
# "-Wno-sign-compare"
# "-Wno-sign-compare",
"-fno-sized-deallocation"
]
if "teensy" not in env["PIOENV"]:
cxxflags += ["-Wno-register"]

2
ini/features.ini
View file

@ -308,7 +308,7 @@ HAS_DUPLICATION_MODE = build_src_filter=+<src/gcode/control/M6
SPI_FLASH_BACKUP = build_src_filter=+<src/gcode/control/M993_M994.cpp>
PLATFORM_M997_SUPPORT = build_src_filter=+<src/gcode/control/M997.cpp>
HAS_TOOLCHANGE = build_src_filter=+<src/gcode/control/T.cpp>
FT_MOTION = build_src_filter=+<src/module/ft_motion.cpp> +<src/gcode/feature/ft_motion>
FT_MOTION = build_src_filter=+<src/module/ft_motion.cpp> +<src/module/ft_motion> +<src/gcode/feature/ft_motion>
LIN_ADVANCE = build_src_filter=+<src/gcode/feature/advance>
PHOTO_GCODE = build_src_filter=+<src/gcode/feature/camera>
CONTROLLER_FAN_EDITABLE = build_src_filter=+<src/gcode/feature/controllerfan>

1
platformio.ini
View file

@ -94,6 +94,7 @@ default_src_filter = +<src/*> -<src/config> -<src/tests>
; Modules
-<src/module>
-<src/module/stepper>
-<src/module/ft_motion>
; Media Support
-<src/sd>
;