diff --git a/Marlin/src/module/ft_motion/trajectory_generator.h b/Marlin/src/module/ft_motion/trajectory_generator.h index 677249af0d..f0d07f932e 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, const float nominal_speed, const float distance) = 0; + virtual void plan(const float initial_speed, const float final_speed, const float acceleration, 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 91ee23b094..e59c8a79fd 100644 --- a/Marlin/src/module/ft_motion/trajectory_poly5.h +++ b/Marlin/src/module/ft_motion/trajectory_poly5.h @@ -34,51 +34,52 @@ class Poly5TrajectoryGenerator : public TrajectoryGenerator { public: Poly5TrajectoryGenerator() = default; - void plan(const float initial_speed, const float final_speed, const float acceleration, const float nominal_speed, const float distance) override { + 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->nominal_speed = nominal_speed; // 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(this->initial_speed) + sq(final_speed)); + const float ldiff = distance + 0.5f * one_over_acc * (sq(initial_speed) + sq(final_speed)); - T2 = ldiff / this->nominal_speed - one_over_acc * this->nominal_speed; + T2 = ldiff / nominal_speed - one_over_acc * nominal_speed; if (T2 < 0.0f) { T2 = 0.0f; - this->nominal_speed = sqrtf(ldiff * acceleration); + nominal_speed = SQRT(ldiff * acceleration); } - T1 = (this->nominal_speed - this->initial_speed) * one_over_acc; - T3 = (this->nominal_speed - final_speed) * one_over_acc; + this->nominal_speed = nominal_speed; - const float d1 = (this->initial_speed + this->nominal_speed) * T1 * 0.5f; + T1 = (nominal_speed - initial_speed) * one_over_acc; + T3 = (nominal_speed - final_speed) * one_over_acc; + + const float d1 = (initial_speed + nominal_speed) * T1 * 0.5f; const float T1_2 = sq(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_c1 = 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; + acc_c3 = (10.0f * d1 - (6.0f * initial_speed + 4.0f * nominal_speed) * T1) / T1_3; + acc_c4 = (15.0f * d1 - (8.0f * initial_speed + 7.0f * nominal_speed) * T1) / -T1_4; + 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 + this->nominal_speed * T2; + pos_after_coast = pos_before_coast + nominal_speed * T2; // Deceration phase - const float d3 = (this->nominal_speed + final_speed) * T3 * 0.5f; + const float d3 = (nominal_speed + final_speed) * T3 * 0.5f; const float T3_2 = sq(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_c1 = 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; + dec_c3 = (10.0f * d3 - (6.0f * nominal_speed + 4.0f * final_speed) * T3) / T3_3; + dec_c4 = (15.0f * d3 - (8.0f * nominal_speed + 7.0f * final_speed) * T3) / -T3_4; + dec_c5 = (6.0f * d3 - 3.0f * (nominal_speed + final_speed) * T3) / T3_5; } void planRunout(const float duration) override { @@ -90,7 +91,8 @@ public: 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 + T2)) { // Coasting phase return pos_before_coast + this->nominal_speed * (t - T1); } diff --git a/Marlin/src/module/ft_motion/trajectory_poly6.cpp b/Marlin/src/module/ft_motion/trajectory_poly6.cpp index 338c7b0d5d..45dff71084 100644 --- a/Marlin/src/module/ft_motion/trajectory_poly6.cpp +++ b/Marlin/src/module/ft_motion/trajectory_poly6.cpp @@ -32,24 +32,25 @@ 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)); + const float ldiff = distance + 0.5f * invA * (sq(initial_speed) + sq(final_speed)); - T2 = ldiff / this->nominal_speed - invA * this->nominal_speed; + T2 = ldiff / nominal_speed - invA * nominal_speed; if (T2 < 0.0f) { T2 = 0.0f; - this->nominal_speed = sqrtf(ldiff * acceleration); + nominal_speed = SQRT(ldiff * acceleration); } - T1 = (this->nominal_speed - this->initial_speed) * invA; - T3 = (this->nominal_speed - final_speed) * invA; + 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 * (this->initial_speed + this->nominal_speed) * T1; - pos_after_coast = pos_before_coast + this->nominal_speed * T2; + pos_before_coast = 0.5f * (initial_speed + nominal_speed) * T1; + pos_after_coast = pos_before_coast + 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, @@ -63,9 +64,9 @@ void Poly6TrajectoryGenerator::plan(const float initial_speed, const float final { const float Ts = T1; const float s0 = 0.0f; - const float v0 = this->initial_speed; + const float v0 = initial_speed; const float s1 = pos_before_coast; - const float v1 = this->nominal_speed; + const float v1 = nominal_speed; const float delta_p = s1 - s0 - v0 * Ts; const float delta_v = (v1 - v0) * Ts; @@ -86,8 +87,8 @@ void Poly6TrajectoryGenerator::plan(const float initial_speed, const float final { 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 v0 = nominal_speed; + const float s1 = pos_after_coast + 0.5f * (nominal_speed + final_speed) * T3; const float v1 = final_speed; const float delta_p = s1 - s0 - v0 * Ts; diff --git a/Marlin/src/module/ft_motion/trajectory_trapezoidal.h b/Marlin/src/module/ft_motion/trajectory_trapezoidal.h index 1c96618f50..2bfe1dc5ed 100644 --- a/Marlin/src/module/ft_motion/trajectory_trapezoidal.h +++ b/Marlin/src/module/ft_motion/trajectory_trapezoidal.h @@ -33,9 +33,8 @@ class TrapezoidalTrajectoryGenerator : public TrajectoryGenerator { public: TrapezoidalTrajectoryGenerator() = default; - void plan(const float initial_speed, const float final_speed, const float acceleration, const float nominal_speed, const float distance) override { + 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->nominal_speed = nominal_speed; this->acceleration = acceleration; const float one_over_accel = 1.0f / acceleration; @@ -44,26 +43,29 @@ public: T2 = ldiff / nominal_speed - one_over_accel * nominal_speed; if (T2 < 0.0f) { T2 = 0.0f; - this->nominal_speed = sqrtf(ldiff * acceleration); + nominal_speed = SQRT(ldiff * acceleration); } - T1 = (this->nominal_speed - initial_speed) * one_over_accel; - T3 = (this->nominal_speed - final_speed) * one_over_accel; + this->nominal_speed = nominal_speed; + + T1 = (nominal_speed - initial_speed) * one_over_accel; + T3 = (nominal_speed - final_speed) * one_over_accel; // Calculate the distance traveled during the accel phase pos_before_coast = initial_speed * T1 + 0.5f * acceleration * sq(T1); // Calculate the distance traveled during the coast phase - pos_after_coast = pos_before_coast + this->nominal_speed * T2; + pos_after_coast = pos_before_coast + nominal_speed * T2; } float getDistanceAtTime(const float t) const override { if (t < T1) { // Acceleration phase return (this->initial_speed * t) + (0.5f * this->acceleration * sq(t)); - } else if (t <= (T1 + T2)) { + } + else if (t <= (T1 + T2)) { // Coasting phase - return pos_before_coast + this->nominal_speed * (t - T1); + return pos_before_coast + nominal_speed * (t - T1); } // Deceleration phase const float tau_decel = t - (T1 + T2);