Add calculateOptimalRotation

src/main/java/frc/robot/swerve/SwerveConstants.java

@@ -47,6 +47,15 @@ public class SwerveConstants {
                 DRIVE_KP, DRIVE_KI, DRIVE_KD, DRIVE_KV, DRIVE_KS, DRIVE_KA, ANGLE_KP, ANGLE_KI,
                 ANGLE_KD, ANGLE_KS
             };
+    public static final LoggedTunableNumber ROTATION_KP =
+            new LoggedTunableNumber("Swerve Drive/Rotation/rotationKP");
+    public static final LoggedTunableNumber ROTATION_KI =
+            new LoggedTunableNumber("Swerve Drive/Rotation/rotationKI");
+    public static final LoggedTunableNumber ROTATION_KD =
+            new LoggedTunableNumber("Swerve Drive/Rotation/rotationKD");
+    public static final LoggedTunableNumber ROTATION_KDIETER =
+            new LoggedTunableNumber("Swerve Drive/Rotation/rotationKDIETER");
+
     public static final VoltageConfigs VOLTAGE_CONFIGS =
             new VoltageConfigs()
                     .withPeakForwardVoltage(VOLT_COMP_SATURATION)
@@ -99,6 +108,11 @@ public static void initConstants(boolean isWCP, boolean isReal) {
             ANGLE_KD.initDefault(0.0);
             ANGLE_KS.initDefault(0.0);
 
+            ROTATION_KP.initDefault(0.9);
+            ROTATION_KI.initDefault(0.0);
+            ROTATION_KD.initDefault(0.0);
+            ROTATION_KDIETER.initDefault(0.0);
+
             ROBOT_WIDTH = 0.584;
             ROBOT_LENGTH = 0.584;
             WHEEL_DIAMETER = 0.099;
@@ -126,6 +140,11 @@ public static void initConstants(boolean isWCP, boolean isReal) {
                 ANGLE_KD.initDefault(0.0);
                 ANGLE_KS.initDefault(0.28);
 
+                ROTATION_KP.initDefault(0.9);
+                ROTATION_KI.initDefault(0.0);
+                ROTATION_KD.initDefault(0.0);
+                ROTATION_KDIETER.initDefault(0.0);
+
                 ROBOT_WIDTH = 0.584;
                 ROBOT_LENGTH = 0.584;
                 WHEEL_DIAMETER = 0.099;
@@ -152,6 +171,11 @@ public static void initConstants(boolean isWCP, boolean isReal) {
                 ANGLE_KD.initDefault(0.0);
                 ANGLE_KS.initDefault(0.000_65);
 
+                ROTATION_KP.initDefault(0.9);
+                ROTATION_KI.initDefault(0.0);
+                ROTATION_KD.initDefault(0.0);
+                ROTATION_KDIETER.initDefault(0.0);
+
                 ROBOT_WIDTH = 0.512;
                 ROBOT_LENGTH = 0.67;
                 WHEEL_DIAMETER = 0.099;

src/main/java/frc/robot/swerve/SwerveDrive.java

@@ -1,6 +1,7 @@
 package frc.robot.swerve;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -19,6 +20,7 @@
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import java.util.stream.Stream;
+import lib.controllers.DieterController;
 import lib.math.differential.Derivative;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -258,6 +260,23 @@ public Command driveCommand(
                                 fieldOriented.getAsBoolean()));
     }
 
+    public Command turnCommand(double rotation, double turnTolerance) {
+        PIDController turnController =
+                new DieterController(
+                        SwerveConstants.ROTATION_KP.get(),
+                        SwerveConstants.ROTATION_KI.get(),
+                        SwerveConstants.ROTATION_KD.get(),
+                        SwerveConstants.ROTATION_KDIETER.get());
+        turnController.setTolerance(turnTolerance);
+        return run(() ->
+                        drive(
+                                0,
+                                0,
+                                turnController.calculate(getYaw().getRotations(), rotation),
+                                false))
+                .until(turnController::atSetpoint);
+    }
+
     public void updateSwerveInputs() {
         for (int i = 0; i < modules.length; i++) {
             loggerInputs.absolutePositions[i] = modules[i].getPosition();

src/main/java/lib/Utils.java

@@ -53,11 +53,18 @@ public static double getDistanceFromPoint(Translation2d point, Pose2d robotPose)
         return robotPose.getTranslation().getDistance(point);
     }
 
-    public static Pose2d calcOptimalPose(List<Pose2d> points, Pose2d robotPose) {
+    public static Pose2d calcClosestPose(List<Pose2d> points, Pose2d robotPose) {
         return points.stream()
                 .min(
                         Comparator.comparingDouble(
                                 point -> getDistanceFromPoint(point.getTranslation(), robotPose)))
                 .orElse(null);
     }
+
+    public static Rotation2d calcRotationToTranslation(
+            Translation2d currentTranslation, Translation2d destinationTranslation) {
+        return new Rotation2d(
+                destinationTranslation.getX() - currentTranslation.getX(),
+                destinationTranslation.getY() - currentTranslation.getY());
+    }
 }