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] Add Translation2d/Translation3d slew rate limiter (#7806)

Browse Source 
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Adrien Bourdeaux
2025-02-25 22:06:00 -05:00
committed by GitHub
parent cd6fee7fea
commit 75321f1d84
4 changed files with 240 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

61
wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
View File

@@ -4,6 +4,9 @@
package edu.wpi.first.math;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.math.geometry.Translation3d;
/** Math utility functions. */
public final class MathUtil {
private MathUtil() {
@@ -209,4 +212,62 @@ public final class MathUtil {
double error = MathUtil.inputModulus(expected - actual, -errorBound, errorBound);
return Math.abs(error) < tolerance;
}
/**
* Limits translation velocity.
*
* @param current Translation at current timestep.
* @param next Translation at next timestep.
* @param dt Timestep duration.
* @param maxVelocity Maximum translation velocity.
* @return Returns the next Translation2d limited to maxVelocity
*/
public static Translation2d slewRateLimit(
Translation2d current, Translation2d next, double dt, double maxVelocity) {
if (maxVelocity < 0) {
Exception e = new IllegalArgumentException();
MathSharedStore.reportError(
"maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
return next;
}
Translation2d diff = next.minus(current);
double dist = diff.getNorm();
if (dist < 1e-9) {
return next;
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
return current.plus(diff.times(maxVelocity * dt / dist));
}
return next;
}
/**
* Limits translation velocity.
*
* @param current Translation at current timestep.
* @param next Translation at next timestep.
* @param dt Timestep duration.
* @param maxVelocity Maximum translation velocity.
* @return Returns the next Translation3d limited to maxVelocity
*/
public static Translation3d slewRateLimit(
Translation3d current, Translation3d next, double dt, double maxVelocity) {
if (maxVelocity < 0) {
Exception e = new IllegalArgumentException();
MathSharedStore.reportError(
"maxVelocity must be a non-negative number, got " + maxVelocity, e.getStackTrace());
return next;
}
Translation3d diff = next.minus(current);
double dist = diff.getNorm();
if (dist < 1e-9) {
return next;
}
if (dist > maxVelocity * dt) {
// Move maximum allowed amount in direction of the difference
return current.plus(diff.times(maxVelocity * dt / dist));
}
return next;
}
}