Maintain robot heading during teleop using PID

src/main/java/frc/robot/controller/DriveController.java

@@ -10,7 +10,6 @@ public class DriveController extends ButtonController {
   private final LogitechF310DirectInputController controller;
   private final SlewRateLimiter xLimiter = new SlewRateLimiter(7);
   private final SlewRateLimiter yLimiter = new SlewRateLimiter(7);
-  private final SlewRateLimiter thetaLimiter = new SlewRateLimiter(7);
 
   public DriveController(LogitechF310DirectInputController controller) {
     super(controller);
@@ -35,6 +34,7 @@ public double getForwardPercentage() {
 
   /** The rotation about the robot's z-axis as a percentage (<code>-1 <= x <= 1</code>) */
   public double getThetaPercentage() {
-    return joystickScale(thetaLimiter.calculate(controller.getRightX()));
+    // This is not ratelimited since heading control is already motion profiled
+    return joystickScale(controller.getRightX());
   }
 }

src/main/java/frc/robot/drive/DriveSubsystem.java

@@ -165,13 +165,28 @@ private void driveTeleop(
       double forwardPercentage,
       double thetaPercentage,
       Rotation2d robotHeading) {
+    final var goalHeadingDifferential =
+        TeleopDriveCommand.MAX_TELEOP_TURN_RATE
+            .times(Constants.PERIOD_SECONDS)
+            .times(-thetaPercentage);
+    final var newGoalHeading =
+        thetaController.getGoal().position + goalHeadingDifferential.getRadians();
+
+    final var thetaControllerVelocity =
+        thetaController.calculate(imuSubsystem.getRotation().getRadians(), newGoalHeading);
     final var chassisSpeeds =
         ChassisSpeeds.fromFieldRelativeSpeeds(
             forwardPercentage * MAX_VELOCITY,
             -sidewaysPercentage * MAX_VELOCITY,
-            thetaPercentage * MAX_ANGULAR_VELOCITY,
+            thetaControllerVelocity,
             robotHeading);
 
+    // TODO: Stop logging this after debugging is finished
+    Logger.getInstance()
+        .recordOutput("Drive/ThetaControllerVelocityRadiansPerSecond", thetaControllerVelocity);
+    Logger.getInstance()
+        .recordOutput("Drive/GoalHeadingRadians", thetaController.getGoal().position);
+
     setChassisSpeeds(chassisSpeeds);
   }