Swerve auto turn

.gitignore

@@ -186,4 +186,9 @@ compile_commands.json
 # Eclipse generated file for annotation processors
 .factorypath
 
-src/main/java/frc/robot/BuildConstants.java
+# Manually added
+src/main/java/frc/robot/BuildConstants.java
+simgui-ds.json
+simgui.json
+/simlogs
+networktables.json

build.gradle

@@ -50,6 +50,10 @@ wpi.java.debugJni = false
 // Set this to true to enable desktop support.
 def includeDesktopSupport = true
 
+<<<<<<< HEAD
+
+=======
+>>>>>>> main
 task(replayWatch, type: JavaExec) {
     mainClass = "org.littletonrobotics.junction.ReplayWatch"
     classpath = sourceSets.main.runtimeClasspath

src/main/java/frc/robot/RobotContainer.java

@@ -67,6 +67,8 @@ private void configureDrive() {
                 driverController::getLeftY,
                 driverController::getLeftX,
                 driverController::getRightX,
+                () -> 0.0,
+                driverController.y(),
                 driverController.leftBumper().negate()::getAsBoolean,
                 driverController.rightBumper()::getAsBoolean);
 

src/main/java/frc/robot/subsystems/swerve/PoseEstimator.java

@@ -0,0 +1,65 @@
+package frc.robot.subsystems.swerve;
+
+import java.util.Optional;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import team2679.atlantiskit.logfields.LogFieldsTable;
+
+public class PoseEstimator {
+    private static final PoseEstimator instance = new PoseEstimator();
+
+    private Pose2d odomertryPose = Pose2d.kZero;
+    private Pose2d estimatedPose = Pose2d.kZero;
+
+    private LogFieldsTable fieldsTable = new LogFieldsTable("PoseEstimator");
+
+    private SwerveModulePosition[] lastModulePositions = new SwerveModulePosition[] {
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+        new SwerveModulePosition(),
+    };
+
+    private PoseEstimator() { }
+
+    public static PoseEstimator getInstance() {
+        return instance;
+    }
+
+    public void update(SwerveDriveKinematics kinematics, SwerveModulePosition[] modulePositions, Optional<Rotation2d> gyroAngle) {
+        Twist2d twist2d = kinematics.toTwist2d(lastModulePositions, modulePositions);
+        lastModulePositions = modulePositions;
+        Pose2d lastOdometryPose = odomertryPose;
+        odomertryPose = odomertryPose.exp(twist2d);
+        if (gyroAngle.isPresent()) {
+            odomertryPose = new Pose2d(odomertryPose.getTranslation(), gyroAngle.get());
+        }
+        fieldsTable.recordOutput("Current Odomertry Pose", odomertryPose);
+        Twist2d twistFromLastPose = lastOdometryPose.log(odomertryPose);
+        estimatedPose = estimatedPose.exp(twistFromLastPose);
+        fieldsTable.recordOutput("Current Estimated Pose", estimatedPose);
+    }
+
+    public void resetPose(Pose2d newPose) {
+        odomertryPose = newPose;
+        estimatedPose = newPose;
+        fieldsTable.recordOutput("Current Odomertry Pose", odomertryPose);
+        fieldsTable.recordOutput("Current Estimated Pose", estimatedPose);
+    }
+
+    public void resetYaw(Rotation2d newYaw) {
+        resetPose(new Pose2d(estimatedPose.getTranslation(), newYaw));
+    }
+
+    public Pose2d getEstimatedPose() {
+        return estimatedPose;
+    }
+
+    public Pose2d getOdometryPose() {
+        return odomertryPose;
+    }
+}

src/main/java/frc/robot/subsystems/swerve/Swerve.java

@@ -66,7 +66,9 @@ public Swerve() {
     isRedAlliance.addOption("blue", false);
 
     isRedAlliance.getSendableChooser().onChange((str) -> {
-      resetYaw(str == "red" ? 0 : 180);
+      double newAngleDegreesCCW = str == "red" ? 0 : 180;
+      resetYaw(newAngleDegreesCCW);
+      PoseEstimator.getInstance().resetYaw(Rotation2d.fromDegrees(newAngleDegreesCCW));
     });
 
     gyroYawDegreesCCW = new RotationalSensorHelper(gyroIO.angleDegreesCCW.getAsDouble());

src/main/java/frc/robot/subsystems/swerve/SwerveCommands.java

@@ -21,9 +21,11 @@ public SwerveCommands(Swerve swerve) {
   }
 
   public TunableCommand driverController(DoubleSupplier forwardSupplier, DoubleSupplier sidewaysSupplier,
-      DoubleSupplier rotationSupplier, BooleanSupplier isFieldRelativeSupplier, BooleanSupplier isSensetiveMode) {
+      DoubleSupplier rotationSupplier, DoubleSupplier yawAutoRotationSupplier,
+      BooleanSupplier autoRotationMode, BooleanSupplier isFieldRelativeSupplier, BooleanSupplier isSensetiveMode) {
 
     return new SwerveDriverController(swerve, forwardSupplier, sidewaysSupplier, rotationSupplier,
+        yawAutoRotationSupplier, autoRotationMode,
         isFieldRelativeSupplier, isSensetiveMode);
   }
 

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

@@ -55,7 +55,7 @@ public final class Sim {
         public static final double DRIVE_MOTOR_MOMENT_OF_INERTIA = 0.025;
         public static final double TURN_MOTOR_MOMENT_OF_INERTIA = 0.004;
 
-        public static final double SIM_TURN_MOTOR_KP = 0;
+        public static final double SIM_TURN_MOTOR_KP = 1.8 * 12;
         public static final double SIM_TURN_MOTOR_KI = 0;
         public static final double SIM_TURN_MOTOR_KD = 0;
     }
@@ -78,5 +78,8 @@ public static final class DriverController {
         public static final double DRIVER_ANGULAR_ACCELERATION_LIMIT_RPS = 4.5;
         public static final double SENSETIVE_TRANSLATION_MULTIPLIER = 0.3;
         public static final double SENSETIVE_ROTATION_MULTIPLIER = 0.3;
+        public static final double ROTATION_KP = 1.8 * 12;
+        public static final double ROTATION_KI = 0;
+        public static final double ROTATION_KD = 0;
     }
 }

src/main/java/frc/robot/subsystems/swerve/SwerveDriverController.java

@@ -3,6 +3,7 @@
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import team2679.atlantiskit.tunables.SendableType;
@@ -16,30 +17,37 @@
 
 public class SwerveDriverController extends TunableCommand {
   private final Swerve swerve;
-  private TunablesTable tuneablesTable = new TunablesTable(SendableType.LIST);
+  private TunablesTable tunablesTable = new TunablesTable(SendableType.LIST);
 
   private DoubleSupplier sidewaysSupplier;
   private DoubleSupplier forwardSupplier;
   private DoubleSupplier rotationsSupplier;
+  private DoubleSupplier yawAutoRotationSupplier;
+  private BooleanSupplier autoRotationMode;
   private BooleanSupplier isFieldRelative;
   private BooleanSupplier isSensetiveMode;
 
-  private DoubleHolder maxAngularVelocityRPS = tuneablesTable.addNumber("Max Angular Velocity RPS",
-      DRIVER_MAX_ANGULAR_VELOCITY_RPS);
+  private DoubleHolder maxAngularVelocityRPS = tunablesTable.addNumber("Max Angular Velocity RPS",
+    DRIVER_MAX_ANGULAR_VELOCITY_RPS);
   private SendableChooser<Double> velocityMultiplierChooser = new SendableChooser<>();
 
   private final SlewRateLimiter forwardSlewRateLimiter = new SlewRateLimiter(DRIVER_ACCELERATION_LIMIT_MPS);
   private final SlewRateLimiter sidewaysSlewRateLimiter = new SlewRateLimiter(DRIVER_ACCELERATION_LIMIT_MPS);
   private final SlewRateLimiter rotationSlewRateLimiter = new SlewRateLimiter(DRIVER_ANGULAR_ACCELERATION_LIMIT_RPS);
 
+  private final PIDController autoRotationPID = new PIDController(ROTATION_KP, ROTATION_KI, ROTATION_KD);
+
   public SwerveDriverController(Swerve swerve, DoubleSupplier forwardSupplier, DoubleSupplier sidewaysSupplier,
-      DoubleSupplier rotationsSupplier, BooleanSupplier isFieldRelative, BooleanSupplier isSensetiveMode) {
+      DoubleSupplier rotationsSupplier, DoubleSupplier yawAutoRotationSupplier, BooleanSupplier autoRotationMode,
+      BooleanSupplier isFieldRelative, BooleanSupplier isSensetiveMode) {
     this.swerve = swerve;
     addRequirements(swerve);
 
     this.sidewaysSupplier = sidewaysSupplier;
     this.forwardSupplier = forwardSupplier;
     this.rotationsSupplier = rotationsSupplier;
+    this.yawAutoRotationSupplier = yawAutoRotationSupplier;
+    this.autoRotationMode = autoRotationMode;
     this.isFieldRelative = isFieldRelative;
     this.isSensetiveMode = isSensetiveMode;
 
@@ -48,7 +56,10 @@ public SwerveDriverController(Swerve swerve, DoubleSupplier forwardSupplier, Dou
     velocityMultiplierChooser.addOption("BABY (30%)", 0.3);
     velocityMultiplierChooser.addOption("EGG (10%)", 0.1);
 
-    tuneablesTable.addChild("velocity chooser", velocityMultiplierChooser);
+    tunablesTable.addChild("velocity chooser", velocityMultiplierChooser);
+
+    autoRotationPID.enableContinuousInput(-Math.PI, Math.PI);
+    tunablesTable.addChild("Auto Rotation PID Controller", autoRotationPID);
   }
 
   @Override
@@ -61,24 +72,33 @@ public void execute() {
 
     double precentageForward = forwardSupplier.getAsDouble() * velocityMultiplier;
     double precentageSideways = sidewaysSupplier.getAsDouble() * velocityMultiplier;
-    double precentageRotation = rotationsSupplier.getAsDouble() * velocityMultiplier;
+    double precentageRotation;
+
+    if (!autoRotationMode.getAsBoolean()) {
+      precentageRotation = rotationsSupplier.getAsDouble() * velocityMultiplier;
+    } else {
+      double currentYawAngle = PoseEstimator.getInstance().getEstimatedPose().getRotation().getDegrees();
+      precentageRotation = autoRotationPID.calculate(currentYawAngle, yawAutoRotationSupplier.getAsDouble())
+        * velocityMultiplier;
+    }
 
     if (isSensetiveMode.getAsBoolean()) {
       precentageForward *= SENSETIVE_TRANSLATION_MULTIPLIER;
       precentageSideways *= SENSETIVE_TRANSLATION_MULTIPLIER;
       precentageRotation *= SENSETIVE_ROTATION_MULTIPLIER;
     }
-    
+
     swerve.drive(
-        forwardSlewRateLimiter.calculate(precentageForward * MAX_VOLTAGE),
-        sidewaysSlewRateLimiter.calculate(precentageSideways * MAX_VOLTAGE),
-        rotationSlewRateLimiter.calculate(precentageRotation * maxAngularVelocityRPS.get()),
-        isFieldRelative.getAsBoolean(),
-        false);
+      forwardSlewRateLimiter.calculate(precentageForward * MAX_VOLTAGE),
+      sidewaysSlewRateLimiter.calculate(precentageSideways * MAX_VOLTAGE),
+      rotationSlewRateLimiter.calculate(precentageRotation * maxAngularVelocityRPS.get()),
+      isFieldRelative.getAsBoolean(),
+      false);
   }
 
   @Override
   public void end(boolean interrupted) {
+    autoRotationPID.reset();
   }
 
   @Override
@@ -88,6 +108,6 @@ public boolean isFinished() {
 
   @Override
   public void initTunable(TunableBuilder builder) {
-    tuneablesTable.initTunable(builder);
+    tunablesTable.initTunable(builder);
   }
 }

src/main/java/frc/robot/subsystems/swerve/io/SwerveModuleSim.java

@@ -35,17 +35,19 @@ protected void periodicBeforeFields() {
         driveMotor.update(0.2);
         turnMotor.update(0.2);
 
-        angleRotaions += (turnMotor.getAngularVelocityRPM() / 60 * 0.02);
-        angleRotaions = warpAngle(angleRotaions);
+        driveMotorRotations += driveMotor.getAngularVelocityRPM() / 60 * 0.02;
+
+        angleRotaions += turnMotor.getAngularVelocityRPM() / 60 * 0.02;
+        turnMotor.setInputVoltage(turnPIDController.calculate(getAbsoluteTurnAngleRotations()));
     }
 
-    private double warpAngle(double angle) {
+    private double wrapAngle(double angle) {
         return ((angle + 1) % 2 + 2) % 2 - 1;
     }
 
     @Override
     protected double getAbsoluteTurnAngleRotations() {
-        return angleRotaions;
+        return wrapAngle(angleRotaions);
     }
 
     @Override
@@ -65,7 +67,7 @@ public void setDrivePercentageSpeed(double speed) {
 
     @Override
     public void setTurnAngleRotations(double rotations) {
-        turnMotor.setInputVoltage(rotations);
+        turnPIDController.setSetpoint(wrapAngle(rotations));
     }
 
     @Override
@@ -75,10 +77,12 @@ protected double getDriveDistanceRotations() {
 
     @Override
     public void setCoast() {
+        driveMotor.setInputVoltage(0);
     }
 
     @Override
     public void resetIntegratedAngleRotations(double newAngle) {
+        angleRotaions = newAngle;
     }
 
     @Override