⚡️ FTM: Reuse trapezoid code in other traj. generators (#28249)

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) {

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.

Marlin/src/module/ft_motion/trajectory_poly5.h

@@ -21,7 +21,7 @@
  */
 #pragma once
 
-#include "trajectory_generator.h"
+#include "trajectory_trapezoidal.h"
 #include <math.h>
 
 /**
@@ -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;
 };

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

Marlin/src/module/ft_motion/trajectory_poly6.h

@@ -21,7 +21,7 @@
  */
 #pragma once
 
-#include "trajectory_generator.h"
+#include "trajectory_trapezoidal.h"
 #include <math.h>
 
 /**
@@ -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;
 };

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²]