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

🐛 FT Motion trajectory fixes (#28101)

Browse source
This commit is contained in:
David Buezas 2025-10-08 02:50:16 +02:00 • committed by GitHub
parent 991fc9cf63
commit 4ddf626d46
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
2 changed files with 13 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -75,7 +75,7 @@ float FTMotion::tau = 0.0f; // (s) Time since start of b
TrapezoidalTrajectoryGenerator FTMotion::trapezoidalGenerator;
Poly5TrajectoryGenerator FTMotion::poly5Generator;
Poly6TrajectoryGenerator FTMotion::poly6Generator;
TrajectoryGenerator& FTMotion::currentGenerator = FTMotion::trapezoidalGenerator;
TrajectoryGenerator* FTMotion::currentGenerator = &FTMotion::trapezoidalGenerator;
TrajectoryType FTMotion::trajectoryType = TrajectoryType::FTM_TRAJECTORY_TYPE;
// Make vector variables.
@ -496,7 +496,7 @@ void FTMotion::runoutBlock() {
const float total_duration = max_intervals * FTM_TS + reminder_from_last_block;
// Plan a zero-motion trajectory for runout
currentGenerator.planRunout(total_duration);
currentGenerator->planRunout(total_duration);
blockProcRdy = true; // since ratio is 0, the trajectory positions won't advance in any axis
}
@ -511,7 +511,7 @@ int32_t FTMotion::stepperCmdBuffItems() {
void FTMotion::init() {
update_shaping_params();
TERN_(FTM_SMOOTHING, update_smoothing_params());
setTrajectoryType(TrajectoryType::FTM_TRAJECTORY_TYPE);
setTrajectoryType(cfg.trajectory_type);
reset(); // Precautionary.
}
@ -520,11 +520,11 @@ 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;
case TrajectoryType::TRAPEZOIDAL: currentGenerator = &trapezoidalGenerator; break;
case TrajectoryType::POLY5: currentGenerator = &poly5Generator; break;
case TrajectoryType::POLY6: currentGenerator = &poly6Generator; break;
}
currentGenerator.reset(); // Reset the selected generator
currentGenerator->reset(); // Reset the selected generator
}
// Load / convert block data from planner to fixed-time control variables.
@ -545,7 +545,7 @@ void FTMotion::loadBlockData(block_t * const current_block) {
const float final_speed = mmps * current_block->final_rate; // (mm/s) End feedrate
// Plan the trajectory using the trajectory generator
currentGenerator.plan(initial_speed, final_speed, current_block->acceleration, current_block->nominal_speed, totalLength);
currentGenerator->plan(initial_speed, final_speed, current_block->acceleration, current_block->nominal_speed, totalLength);
// Accel + Coasting + Decel + datapoints
const float reminder_from_last_block = - tau;
@ -555,7 +555,7 @@ void FTMotion::loadBlockData(block_t * const current_block) {
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();
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)));
@ -572,7 +572,7 @@ void FTMotion::loadBlockData(block_t * const current_block) {
// Generate data points of the trajectory.
// Called from FTMotion::loop() at the fetch of a new planner block, after loadBlockData.
void FTMotion::generateTrajectoryPointsFromBlock() {
const float total_duration = currentGenerator.getTotalDuration();
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
@ -589,7 +589,7 @@ void FTMotion::generateTrajectoryPointsFromBlock() {
// tau can start negative, but it always holds that `tau > -FTM_TS`
// Get distance from trajectory generator
const float dist = currentGenerator.getDistanceAtTime(tau);
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);

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

@ -73,7 +73,7 @@ typedef struct FTConfig {
float linearAdvK = FTM_LINEAR_ADV_DEFAULT_K; // Linear advance gain.
#endif
TrajectoryType trajectory_type = TrajectoryType::TRAPEZOIDAL; // Trajectory generator type
TrajectoryType trajectory_type = TrajectoryType::FTM_TRAJECTORY_TYPE; // Trajectory generator type
float poly6_acceleration_overshoot; // Overshoot factor for Poly6 (1.25 to 2.0)
} ft_config_t;
@ -186,7 +186,7 @@ class FTMotion {
static TrapezoidalTrajectoryGenerator trapezoidalGenerator;
static Poly5TrajectoryGenerator poly5Generator;
static Poly6TrajectoryGenerator poly6Generator;
static TrajectoryGenerator& currentGenerator;
static TrajectoryGenerator* currentGenerator;
static TrajectoryType trajectoryType;
// Number of batches needed to propagate the current trajectory to the stepper.