Phoenix6 update

build.gradle

@@ -132,11 +132,6 @@ jar {
     exclude('**/*.java')
     // Exclude tests folder from the JAR
     exclude('**/tests/**')
-//    exclude('frc/robot/Robot.class')
-//    exclude('frc/robot/RobotContainer.class')
-//    exclude('frc/robot/Main.class')
-//    exclude('frc/robot/Constants.class')
-//    exclude('frc/robot/Ports.class')
 }
 
 // Configure jar and deploy tasks

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

@@ -1,17 +1,19 @@
 package frc.robot.swerve;
 
+import edu.wpi.first.math.geometry.Rotation2d;
+
 public interface GyroIO {
     void updateInputs(SwerveDriveInputs inputs);
 
-    double getYaw();
+    Rotation2d getYaw();
 
-    default double getRawYaw() {
-        return 0;
+    default Rotation2d getRawYaw() {
+        return new Rotation2d();
     }
 
-    default double getPitch() {
-        return 0;
+    default Rotation2d getPitch() {
+        return new Rotation2d();
     }
 
-    void resetGyro(double angle);
+    void resetGyro(Rotation2d angle);
 }

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

@@ -2,34 +2,35 @@
 
 import com.kauailabs.navx.frc.AHRS;
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.SPI;
 
 public class GyroIOReal implements GyroIO {
     private final AHRS gyro;
-    private double gyroOffset = 0;
+    private Rotation2d gyroOffset = new Rotation2d();
 
     public GyroIOReal() {
         this.gyro = new AHRS(SPI.Port.kMXP);
     }
 
     @Override
-    public double getYaw() {
-        return getRawYaw() - gyroOffset;
+    public Rotation2d getYaw() {
+        return getRawYaw().minus(gyroOffset);
     }
 
     @Override
-    public double getRawYaw() {
-        return -MathUtil.angleModulus(Math.toRadians(gyro.getAngle()));
+    public Rotation2d getRawYaw() {
+        return new Rotation2d(-MathUtil.angleModulus(Math.toRadians(gyro.getAngle())));
     }
 
     @Override
-    public double getPitch() {
-        return gyro.getPitch() - 2.95;
+    public Rotation2d getPitch() {
+        return new Rotation2d(gyro.getPitch());
     }
 
     @Override
-    public void resetGyro(double angle) {
-        gyroOffset = -angle + getRawYaw();
+    public void resetGyro(Rotation2d angle) {
+        gyroOffset = getRawYaw().minus(angle);
     }
 
     @Override

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

@@ -1,22 +1,23 @@
 package frc.robot.swerve;
 
+import edu.wpi.first.math.geometry.Rotation2d;
 import lib.math.differential.Integral;
 
 public class GyroIOSim implements GyroIO {
     private final Integral yaw = new Integral();
 
     @Override
-    public double getYaw() {
-        return yaw.get();
+    public Rotation2d getYaw() {
+        return new Rotation2d(yaw.get());
     }
 
     @Override
-    public void resetGyro(double angle) {
-        yaw.override(angle);
+    public void resetGyro(Rotation2d angle) {
+        yaw.override(angle.getRadians());
     }
 
     @Override
     public void updateInputs(SwerveDriveInputs inputs) {
-        yaw.update(inputs.currentSpeeds[2]);
+        yaw.update(inputs.currentSpeeds.omegaRadiansPerSecond);
     }
 }