From 99768cffbee9c11fce9434bae878ef53ce07aca3 Mon Sep 17 00:00:00 2001 From: David Buezas Date: Fri, 23 Jan 2026 02:59:37 +0100 Subject: [PATCH] =?UTF-8?q?=E2=9A=A1=EF=B8=8F=20FTM:=20Reuse=20trapezoid?= =?UTF-8?q?=20code=20in=20other=20traj.=20generators=20(#28249)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../module/ft_motion/resonance_generator.h | 2 - .../module/ft_motion/trajectory_generator.h | 2 +- .../src/module/ft_motion/trajectory_poly5.h | 48 +++++------------- .../src/module/ft_motion/trajectory_poly6.cpp | 49 +++++-------------- .../src/module/ft_motion/trajectory_poly6.h | 17 ++----- .../module/ft_motion/trajectory_trapezoidal.h | 38 ++++++++------ 6 files changed, 53 insertions(+), 103 deletions(-) diff --git a/Marlin/src/module/ft_motion/resonance_generator.h b/Marlin/src/module/ft_motion/resonance_generator.h index 06a34e4fd6..a432ee964d 100644 --- a/Marlin/src/module/ft_motion/resonance_generator.h +++ b/Marlin/src/module/ft_motion/resonance_generator.h @@ -46,8 +46,6 @@ class ResonanceGenerator { ResonanceGenerator(); - void planRunout(const float duration); - void reset(); void start(const xyze_pos_t &spos, const float t) { diff --git a/Marlin/src/module/ft_motion/trajectory_generator.h b/Marlin/src/module/ft_motion/trajectory_generator.h index f0d07f932e..677249af0d 100644 --- a/Marlin/src/module/ft_motion/trajectory_generator.h +++ b/Marlin/src/module/ft_motion/trajectory_generator.h @@ -37,7 +37,7 @@ public: * @param nominal_speed Peak feedrate [mm/s] * @param distance Total distance to travel [mm] */ - virtual void plan(const float initial_speed, const float final_speed, const float acceleration, float nominal_speed, const float distance) = 0; + virtual void plan(const float initial_speed, const float final_speed, const float acceleration, const float nominal_speed, const float distance) = 0; /** * Plan a zero-motion trajectory for a specific duration. diff --git a/Marlin/src/module/ft_motion/trajectory_poly5.h b/Marlin/src/module/ft_motion/trajectory_poly5.h index e59c8a79fd..df294e615d 100644 --- a/Marlin/src/module/ft_motion/trajectory_poly5.h +++ b/Marlin/src/module/ft_motion/trajectory_poly5.h @@ -21,7 +21,7 @@ */ #pragma once -#include "trajectory_generator.h" +#include "trajectory_trapezoidal.h" #include /** @@ -30,27 +30,15 @@ * 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 { +class Poly5TrajectoryGenerator : public TrapezoidalTrajectoryGenerator { public: Poly5TrajectoryGenerator() = default; - void plan(const float initial_speed, const float final_speed, const float acceleration, float nominal_speed, const float distance) override { - this->initial_speed = initial_speed; + void plan(const float initial_speed_in, const float final_speed_in, const float acceleration_in, const float nominal_speed_in, const float distance_in) override { + // Use base class to calculate T1, T2, T3, actual nominal (top) speed and basic positions + TrapezoidalTrajectoryGenerator::plan(initial_speed_in, final_speed_in, acceleration_in, nominal_speed_in, distance_in); - // Calculate timing phases using the same logic as trapezoidal generator - const float one_over_acc = 1.0f / acceleration; - const float ldiff = distance + 0.5f * one_over_acc * (sq(initial_speed) + sq(final_speed)); - - T2 = ldiff / nominal_speed - one_over_acc * nominal_speed; - if (T2 < 0.0f) { - T2 = 0.0f; - nominal_speed = SQRT(ldiff * acceleration); - } - - this->nominal_speed = nominal_speed; - - T1 = (nominal_speed - initial_speed) * one_over_acc; - T3 = (nominal_speed - final_speed) * one_over_acc; + const float final_speed = final_speed_in; // just for consistency with the other parameters that otherwise shadow the member variables const float d1 = (initial_speed + nominal_speed) * T1 * 0.5f; const float T1_2 = sq(T1); @@ -65,8 +53,7 @@ public: acc_c5 = (6.0f * d1 - 3.0f * (initial_speed + nominal_speed) * T1) / T1_5; pos_before_coast = d1; - // Coast phase - pos_after_coast = pos_before_coast + nominal_speed * T2; + // Coast phase - already calculated by base class // Deceration phase const float d3 = (nominal_speed + final_speed) * T3 * 0.5f; @@ -82,33 +69,24 @@ public: dec_c5 = (6.0f * d3 - 3.0f * (nominal_speed + final_speed) * T3) / T3_5; } - void planRunout(const float duration) override { - reset(); - T2 = duration; - } - float getDistanceAtTime(const float t) const override { if (t < T1) { // Acceration phase return t * (acc_c1 + sq(t) * (acc_c3 + t * (acc_c4 + t * acc_c5))); } - else if (t <= (T1 + T2)) { + else if (t <= T1_plus_T2) { // Coasting phase - return pos_before_coast + this->nominal_speed * (t - T1); + return pos_before_coast + nominal_speed * (t - T1); } // Deceration phase - const float tau = t - (T1 + T2); + const float tau = t - T1_plus_T2; return pos_after_coast + tau * (dec_c1 + sq(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; + TrapezoidalTrajectoryGenerator::reset(); } private: @@ -117,8 +95,4 @@ private: 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; }; diff --git a/Marlin/src/module/ft_motion/trajectory_poly6.cpp b/Marlin/src/module/ft_motion/trajectory_poly6.cpp index 4693828fe3..d133523101 100644 --- a/Marlin/src/module/ft_motion/trajectory_poly6.cpp +++ b/Marlin/src/module/ft_motion/trajectory_poly6.cpp @@ -30,27 +30,11 @@ 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; +void Poly6TrajectoryGenerator::plan(const float initial_speed_in, const float final_speed_in, const float acceleration_in, const float nominal_speed_in, const float distance_in) { + // Use base class to calculate T1, T2, T3 and basic positions + TrapezoidalTrajectoryGenerator::plan(initial_speed_in, final_speed_in, acceleration_in, nominal_speed_in, distance_in); - // --- Trapezoid timings (unchanged) --- - const float invA = 1.0f / acceleration; - const float ldiff = distance + 0.5f * invA * (sq(initial_speed) + sq(final_speed)); - - T2 = ldiff / nominal_speed - invA * nominal_speed; - if (T2 < 0.0f) { - T2 = 0.0f; - nominal_speed = SQRT(ldiff * acceleration); - } - - this->nominal_speed = nominal_speed; - - T1 = (nominal_speed - initial_speed) * invA; - T3 = (nominal_speed - final_speed) * invA; - - // Distances at phase boundaries (trapezoid areas) - pos_before_coast = 0.5f * (initial_speed + nominal_speed) * T1; - pos_after_coast = pos_before_coast + nominal_speed * T2; + const float final_speed = final_speed_in; // just for consistency with the other parameters that otherwise shadow the member variables // --- 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, @@ -105,11 +89,6 @@ void Poly6TrajectoryGenerator::plan(const float initial_speed, const float final } -void Poly6TrajectoryGenerator::planRunout(const float duration) { - reset(); - T2 = duration; -} - float Poly6TrajectoryGenerator::getDistanceAtTime(const float t) const { if (t < T1) { // Accel phase: u=t/T1 @@ -117,27 +96,25 @@ float Poly6TrajectoryGenerator::getDistanceAtTime(const float t) const { 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)) { + else if (t <= T1_plus_T2) { // Coast - return pos_before_coast + this->nominal_speed * (t - T1); + return pos_before_coast + nominal_speed * (t - T1); } // Decel phase - const float tau = t - (T1 + T2), + const float tau = t - T1_plus_T2, 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); + return s5_u(pos_after_coast, nominal_speed, T3, dec_c3, dec_c4, dec_c5, u) + + dec_c6 * K_u(pos_after_coast, 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; - + // Reset polynomial coefficients acc_c3 = acc_c4 = acc_c5 = 0.0f; dec_c3 = dec_c4 = dec_c5 = 0.0f; acc_c6 = dec_c6 = 0.0f; + + // Call base class reset to handle inherited members + TrapezoidalTrajectoryGenerator::reset(); } #endif // FTM_POLYS diff --git a/Marlin/src/module/ft_motion/trajectory_poly6.h b/Marlin/src/module/ft_motion/trajectory_poly6.h index 6ecc6f064c..61c11dc06d 100644 --- a/Marlin/src/module/ft_motion/trajectory_poly6.h +++ b/Marlin/src/module/ft_motion/trajectory_poly6.h @@ -21,7 +21,7 @@ */ #pragma once -#include "trajectory_generator.h" +#include "trajectory_trapezoidal.h" #include /** @@ -31,18 +31,14 @@ * - 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 { +class Poly6TrajectoryGenerator : public TrapezoidalTrajectoryGenerator { 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; + void plan(const float initial_speed, const float final_speed, const float acceleration, const float nominal_speed, const float distance) override; float getDistanceAtTime(const float t) const override; - float getTotalDuration() const override; - void reset() override; private: @@ -64,12 +60,7 @@ private: return cu(u) * cu(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 + // New phase polynomials (only the polynomial coefficients are specific to Poly6) 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; }; diff --git a/Marlin/src/module/ft_motion/trajectory_trapezoidal.h b/Marlin/src/module/ft_motion/trajectory_trapezoidal.h index 2bfe1dc5ed..6d2d30cfb0 100644 --- a/Marlin/src/module/ft_motion/trajectory_trapezoidal.h +++ b/Marlin/src/module/ft_motion/trajectory_trapezoidal.h @@ -33,9 +33,14 @@ class TrapezoidalTrajectoryGenerator : public TrajectoryGenerator { public: TrapezoidalTrajectoryGenerator() = default; - void plan(const float initial_speed, const float final_speed, const float acceleration, float nominal_speed, const float distance) override { - this->initial_speed = initial_speed; - this->acceleration = acceleration; + void plan(const float initial_speed_in, const float final_speed_in, const float acceleration_in, const float nominal_speed_in, const float distance_in) override { + + initial_speed = initial_speed_in; + acceleration = acceleration_in; + nominal_speed = nominal_speed_in; + const float distance = distance_in; + const float final_speed = final_speed_in; // just for consistency + const float one_over_accel = 1.0f / acceleration; const float ldiff = distance + 0.5f * one_over_accel * (sq(initial_speed) + sq(final_speed)); @@ -46,7 +51,6 @@ public: nominal_speed = SQRT(ldiff * acceleration); } - this->nominal_speed = nominal_speed; T1 = (nominal_speed - initial_speed) * one_over_accel; T3 = (nominal_speed - final_speed) * one_over_accel; @@ -56,42 +60,48 @@ public: // Calculate the distance traveled during the coast phase pos_after_coast = pos_before_coast + nominal_speed * T2; + + // Cache frequently used sums for performance + T1_plus_T2 = T1 + T2; + total_duration = T1_plus_T2 + T3; } float getDistanceAtTime(const float t) const override { if (t < T1) { // Acceleration phase - return (this->initial_speed * t) + (0.5f * this->acceleration * sq(t)); + return (initial_speed * t) + (0.5f * acceleration * sq(t)); } - else if (t <= (T1 + T2)) { + else if (t <= T1_plus_T2) { // Coasting phase return pos_before_coast + 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 * sq(tau_decel); + const float tau_decel = t - T1_plus_T2; + return pos_after_coast + nominal_speed * tau_decel - 0.5f * acceleration * sq(tau_decel); } float getTotalDuration() const override { - return T1 + T2 + T3; + return total_duration; } void planRunout(const float duration) override { reset(); - T2 = duration; // Coast at zero speed for the entire duration + T2 = T1_plus_T2 = total_duration = 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; + T1 = T2 = T3 = T1_plus_T2 = total_duration = 0.0f; + initial_speed = nominal_speed = acceleration = 0.0f; pos_before_coast = pos_after_coast = 0.0f; } -private: - // Internal trajectory parameters - kept private +protected: + // Internal trajectory parameters - made protected for inheritance 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 T1_plus_T2 = 0.0f; // Cached sum of T1 + T2 for performance + float total_duration = 0.0f; // Cached total duration T1 + T2 + T3 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²]