Merge branch 'main' into 2022

2026-06-21 01:01:43 +00:00 · 2021-04-19 18:45:31 -07:00
parent 8d961dfd25 aaf24e2552
commit 05ebe93180
44 changed files with 1762 additions and 702 deletions
--- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
+++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
@@ -72,4 +72,19 @@ class HolonomicDriveControllerTest {
                MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()),
                kAngularTolerance));
  }
+
+  @Test
+  void testDoesNotRotateUnnecessarily() {
+    var controller =
+        new HolonomicDriveController(
+            new PIDController(1, 0, 0),
+            new PIDController(1, 0, 0),
+            new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(4, 2)));
+
+    ChassisSpeeds speeds =
+        controller.calculate(
+            new Pose2d(0, 0, new Rotation2d(1.57)), new Pose2d(), 0, new Rotation2d(1.57));
+
+    assertEquals(0.0, speeds.omegaRadiansPerSecond);
+  }
 }