diff --git a/Marlin/src/module/ft_motion.cpp b/Marlin/src/module/ft_motion.cpp index 219fb1a3f6..d3ecd76630 100644 --- a/Marlin/src/module/ft_motion.cpp +++ b/Marlin/src/module/ft_motion.cpp @@ -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); diff --git a/Marlin/src/module/ft_motion.h b/Marlin/src/module/ft_motion.h index c75f0396fb..11f85aea53 100644 --- a/Marlin/src/module/ft_motion.h +++ b/Marlin/src/module/ft_motion.h @@ -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.