Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Refactor TrapezoidProfile API (#5457)

Browse Source
This commit is contained in:
Gold856
2023-07-19 20:25:10 -04:00
committed by GitHub
parent 72a4543493
commit 86e91e6724
24 changed files with 492 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
View File

@@ -343,8 +343,8 @@ public class ProfiledPIDController implements Sendable {
m_setpoint.position = setpointMinDistance + measurement;
}
var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
m_setpoint = profile.calculate(getPeriod());
var profile = new TrapezoidProfile(m_constraints);
m_setpoint = profile.calculate(getPeriod(), m_goal, m_setpoint);
return m_controller.calculate(measurement, m_setpoint.position);
}

126
wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
View File

@@ -22,14 +22,14 @@ import java.util.Objects;
* new TrapezoidProfile.Constraints(kMaxV, kMaxA);
* TrapezoidProfile.State previousProfiledReference =
* new TrapezoidProfile.State(initialReference, 0.0);
* TrapezoidProfile profile = new TrapezoidProfile(constraints);
* </code></pre>
*
* <p>Run on update:
*
* <pre><code>
* TrapezoidProfile profile =
* new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
* previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
* previousProfiledReference =
* profile.calculate(timeSincePreviousUpdate, unprofiledReference, previousProfiledReference);
* </code></pre>
*
* <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
@@ -42,9 +42,10 @@ public class TrapezoidProfile {
// The direction of the profile, either 1 for forwards or -1 for inverted
private int m_direction;
private Constraints m_constraints;
private State m_initial;
private State m_goal;
private final Constraints m_constraints;
private State m_current;
private State m_goal; // TODO: Remove
private final boolean m_newAPI; // TODO: Remove
private double m_endAccel;
private double m_endFullSpeed;
@@ -96,27 +97,40 @@ public class TrapezoidProfile {
}
}
/**
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
*/
public TrapezoidProfile(Constraints constraints) {
m_constraints = constraints;
m_newAPI = true;
}
/**
* Construct a TrapezoidProfile.
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @param initial The initial state (usually the current state).
* @deprecated Pass the desired and current state into calculate instead of constructing a new
* TrapezoidProfile with the desired and current state
*/
@Deprecated(since = "2024", forRemoval = true)
public TrapezoidProfile(Constraints constraints, State goal, State initial) {
m_direction = shouldFlipAcceleration(initial, goal) ? -1 : 1;
m_constraints = constraints;
m_initial = direct(initial);
m_current = direct(initial);
m_goal = direct(goal);
if (m_initial.velocity > m_constraints.maxVelocity) {
m_initial.velocity = m_constraints.maxVelocity;
m_newAPI = false;
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
@@ -126,7 +140,7 @@ public class TrapezoidProfile {
// of a truncated one
double fullTrapezoidDist =
cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
cutoffDistBegin + (m_goal.position - m_current.position) + cutoffDistEnd;
double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
double fullSpeedDist =
@@ -148,7 +162,10 @@ public class TrapezoidProfile {
*
* @param constraints The constraints on the profile, like maximum velocity.
* @param goal The desired state when the profile is complete.
* @deprecated Pass the desired and current state into calculate instead of constructing a new
* TrapezoidProfile with the desired and current state
*/
@Deprecated(since = "2024", forRemoval = true)
public TrapezoidProfile(Constraints constraints, State goal) {
this(constraints, goal, new State(0, 0));
}
@@ -159,17 +176,23 @@ public class TrapezoidProfile {
*
* @param t The time since the beginning of the profile.
* @return The position and velocity of the profile at time t.
* @deprecated Pass the desired and current state into calculate instead of constructing a new
* TrapezoidProfile with the desired and current state
*/
@Deprecated(since = "2024", forRemoval = true)
public State calculate(double t) {
State result = new State(m_initial.position, m_initial.velocity);
if (m_newAPI) {
throw new RuntimeException("Cannot use new constructor with deprecated calculate()");
}
State result = new State(m_current.position, m_current.velocity);
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position += (m_initial.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position +=
(m_initial.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
(m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+ m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDeccel) {
result.velocity = m_goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
@@ -184,6 +207,75 @@ public class TrapezoidProfile {
return direct(result);
}
/**
* Calculate the correct position and velocity for the profile at a time t where the beginning of
* the profile was at time t = 0.
*
* @param t The time since the beginning of the profile.
* @param goal The desired state when the profile is complete.
* @param current The current state.
* @return The position and velocity of the profile at time t.
*/
public State calculate(double t, State goal, State current) {
m_direction = shouldFlipAcceleration(current, goal) ? -1 : 1;
m_current = direct(current);
goal = direct(goal);
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
}
// Deal with a possibly truncated motion profile (with nonzero initial or
// final velocity) by calculating the parameters as if the profile began and
// ended at zero velocity
double cutoffBegin = m_current.velocity / m_constraints.maxAcceleration;
double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
double cutoffEnd = goal.velocity / m_constraints.maxAcceleration;
double cutoffDistEnd = cutoffEnd * cutoffEnd * m_constraints.maxAcceleration / 2.0;
// Now we can calculate the parameters as if it was a full trapezoid instead
// of a truncated one
double fullTrapezoidDist =
cutoffDistBegin + (goal.position - m_current.position) + cutoffDistEnd;
double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
double fullSpeedDist =
fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
// Handle the case where the profile never reaches full speed
if (fullSpeedDist < 0) {
accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
fullSpeedDist = 0;
}
m_endAccel = accelerationTime - cutoffBegin;
m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
State result = new State(m_current.position, m_current.velocity);
if (t < m_endAccel) {
result.velocity += t * m_constraints.maxAcceleration;
result.position += (m_current.velocity + t * m_constraints.maxAcceleration / 2.0) * t;
} else if (t < m_endFullSpeed) {
result.velocity = m_constraints.maxVelocity;
result.position +=
(m_current.velocity + m_endAccel * m_constraints.maxAcceleration / 2.0) * m_endAccel
+ m_constraints.maxVelocity * (t - m_endAccel);
} else if (t <= m_endDeccel) {
result.velocity = goal.velocity + (m_endDeccel - t) * m_constraints.maxAcceleration;
double timeLeft = m_endDeccel - t;
result.position =
goal.position
- (goal.velocity + timeLeft * m_constraints.maxAcceleration / 2.0) * timeLeft;
} else {
result = goal;
}
return direct(result);
}
/**
* Returns the time left until a target distance in the profile is reached.
*
@@ -191,8 +283,8 @@ public class TrapezoidProfile {
* @return The time left until a target distance in the profile is reached.
*/
public double timeLeftUntil(double target) {
double position = m_initial.position * m_direction;
double velocity = m_initial.velocity * m_direction;
double position = m_current.position * m_direction;
double velocity = m_current.velocity * m_direction;
double endAccel = m_endAccel * m_direction;
double endFullSpeed = m_endFullSpeed * m_direction - endAccel;