raphael.breiting/Marlin
1
1
Fork 0
forked from Mirrors/Marlin
Code Pull Requests Activity

🧑‍💻 FTM trajectory code tweaks

Browse Source
This commit is contained in:
Scott Lahteine
2025-11-11 19:50:22 -06:00
parent ba8b685ede
commit b0f6155d99
4 changed files with 45 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Marlin/src/module/ft_motion/trajectory_generator.h
View File

@@ -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.

40
Marlin/src/module/ft_motion/trajectory_poly5.h
View File

@@ -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);
}

25
Marlin/src/module/ft_motion/trajectory_poly6.cpp
View File

@@ -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;

18
Marlin/src/module/ft_motion/trajectory_trapezoidal.h
View File

@@ -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);