Fixed radians.

Signed-off-by: thenetworkgrinch <thenetworkgrinch@users.noreply.github.com>

Author: thenetworkgrinch

src/main/java/swervelib/SwerveInputStream.java

@@ -282,11 +282,11 @@ public SwerveInputStream driveToPose(Supplier<Pose2d> pose, ProfiledPIDControlle
   {
     omegaPIDController.reset(swerveDrive.getPose().getRotation().getRadians());
     xPIDController.reset(swerveDrive.getPose().getTranslation().getDistance(pose.get().getTranslation()));
+    omegaPIDController.enableContinuousInput(-Math.PI, Math.PI);
     xPIDController.setGoal(new State(0, 0));
     driveToPose = Optional.of(pose);
     driveToPoseTranslationPIDController = Optional.of(xPIDController);
     driveToPoseOmegaPIDController = Optional.of(omegaPIDController);
-    if(driveToPoseOmegaPIDController.isPresent()) {driveToPoseOmegaPIDController.get().enableContinuousInput(-Math.PI, Math.PI);}
     return this;
   }