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

[wpimath] Fix TrapezoidProfile limiting velocity incorrectly (#8030)

Browse Source
This commit is contained in:
ThePixelatedCat
2025-06-25 15:36:53 +10:00
committed by GitHub
parent ddc5220ed4
commit ffe296892c
4 changed files with 104 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -60,10 +60,14 @@ public class TrapezoidProfile {
/**
* Constructs constraints for a TrapezoidProfile.
*
* @param maxVelocity maximum velocity
* @param maxAcceleration maximum acceleration
* @param maxVelocity Maximum velocity, must be non-negative.
* @param maxAcceleration Maximum acceleration, must be non-negative.
*/
public Constraints(double maxVelocity, double maxAcceleration) {
if (maxVelocity < 0.0 || maxAcceleration < 0.0) {
throw new IllegalArgumentException("Constraints must be non-negative");
}
this.maxVelocity = maxVelocity;
this.maxAcceleration = maxAcceleration;
MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
@@ -128,8 +132,8 @@ public class TrapezoidProfile {
m_current = direct(current);
goal = direct(goal);
if (m_current.velocity > m_constraints.maxVelocity) {
m_current.velocity = m_constraints.maxVelocity;
if (Math.abs(m_current.velocity) > m_constraints.maxVelocity) {
m_current.velocity = Math.copySign(m_constraints.maxVelocity, m_current.velocity);
}
// Deal with a possibly truncated motion profile (with nonzero initial or