Change turn command to use DIETER controller

Galaxia5987 · Feb 6, 2024 · 40fe1c4 · 40fe1c4
1 parent b3d6ccb
commit 40fe1c4
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/swerve/SwerveConstants.java b/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)

diff --git a/src/main/java/frc/robot/swerve/SwerveDrive.java b/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,7 +20,7 @@
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import java.util.stream.Stream;
-import lib.Utils;
+import lib.controllers.DieterController;
 import lib.math.differential.Derivative;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
@@ -260,8 +261,20 @@ public Command driveCommand(
     }
 
     public Command turnCommand(double rotation, double turnTolerance) {
-        return run(() -> drive(0, 0, rotation, false))
-                .until(() -> Utils.epsilonEquals(rotation, getYaw().getRotations(), 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() {