angular acceleration & velocity constraints

team581 · Jan 3, 2025 · 487e089 · 487e089
1 parent 1f901be
commit 487e089
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 5 deletions.
diff --git a/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java b/src/main/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculator.java
@@ -53,9 +53,14 @@ public static ChassisSpeeds constrainLinearVelocity(
 
   private static ChassisSpeeds constrainRotationalVelocity(
       ChassisSpeeds inputSpeeds, AutoConstraintOptions options) {
-    // TODO: Implement rotational velocity constraint
-    double unconstrainedRotationalVelocity =
-        Math.hypot(inputSpeeds.vxMetersPerSecond, inputSpeeds.vyMetersPerSecond);
+    double currentAngularVelocity = inputSpeeds.omegaRadiansPerSecond;
+    if (currentAngularVelocity > options.maxAngularVelocity()) {
+      double clampingFactor = options.maxAngularVelocity() / currentAngularVelocity;
+      return new ChassisSpeeds(
+          inputSpeeds.vxMetersPerSecond,
+          inputSpeeds.vyMetersPerSecond,
+          inputSpeeds.omegaRadiansPerSecond * clampingFactor);
+    }
 
     return inputSpeeds;
   }
@@ -91,7 +96,21 @@ private static ChassisSpeeds constrainRotationalAcceleration(
       ChassisSpeeds previousSpeeds,
       double timeBetweenPreviousAndInputSpeeds,
       AutoConstraintOptions options) {
-    // TODO: Implement angular acceleration constraint
+
+    double currentAngularSpeed = inputSpeeds.omegaRadiansPerSecond;
+    double previousAngularSpeed = previousSpeeds.omegaRadiansPerSecond;
+
+    double currentAngularAcceleration =
+        currentAngularSpeed - previousAngularSpeed / timeBetweenPreviousAndInputSpeeds;
+    if (currentAngularAcceleration > options.maxAngularAcceleration()) {
+      double constrainedAngularAcceleration =
+          previousAngularSpeed
+              + options.maxAngularAcceleration() * timeBetweenPreviousAndInputSpeeds;
+      return new ChassisSpeeds(
+          inputSpeeds.vxMetersPerSecond,
+          inputSpeeds.vyMetersPerSecond,
+          constrainedAngularAcceleration);
+    }
     return inputSpeeds;
   }
 

diff --git a/src/test/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculatorTest.java b/src/test/java/frc/robot/autos/trailblazer/constraints/AutoConstraintCalculatorTest.java
@@ -125,7 +125,7 @@ void verifyRotationalAccelerationConstraints() {
         new ChassisSpeeds(0.0, 0.0, -10.0),
         new ChassisSpeeds(10.0, 10.0, 10.0),
         new AutoConstraintOptions().withMaxAngularAcceleration(10.0),
-        new ChassisSpeeds(-7.5, 10.0, 10.0));
+        new ChassisSpeeds(10.0, 10.0, -7.5));
   }
 
   /** Verify that all constraints being applied on a complex problem at once works. */