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

[wpilib] Fix initial heading behavior in HolonomicDriveController (#3290)

Browse Source
This commit is contained in:
Prateek Machiraju
2021-04-19 00:00:11 -04:00
committed by GitHub
parent 659b37ef9d
commit aaf24e2552
5 changed files with 47 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
View File

@@ -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);
}
}