From aad75b9b559825aaf60ad5bb550bf8263d3d203d Mon Sep 17 00:00:00 2001
From: Tyler Wilcox <tyler@tywilly.com>
Date: Tue, 24 Mar 2026 20:52:56 -0400
Subject: [PATCH] Fix Swerve Odemetry

---
 .../robolib/drivesystems/swerve/SwerveDriveSystem.java   | 9 +++++----
 .../frc/robolib/drivesystems/swerve/SwerveModule.java    | 2 +-
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
index e87e988..ccd527a 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveDriveSystem.java
@@ -17,6 +17,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.wpilibj2.command.Subsystem;
 
@@ -72,7 +73,7 @@ public class SwerveDriveSystem implements Subsystem{
 
         gyro = new Pigeon2(pigeonId, canBus);
 
-        poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions(), Pose3d.kZero);
+        poseEstimator = new SwerveDrivePoseEstimator3d(driveKiniematics, new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().in(Units.Revolution))), getModulePositions(), Pose3d.kZero);
         chassisSpeeds = new ChassisSpeeds();
     }
 
@@ -84,7 +85,7 @@ public class SwerveDriveSystem implements Subsystem{
             moduleStates[i].optimize(modules[i].getSteerRotations());
         }
 
-        poseEstimator.update(new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
+        poseEstimator.update(new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().in(Units.Rotations))), getModulePositions());
         // poseEstimator.updateWithTime(Utils.getCurrentTimeSeconds(), new Rotation3d(Rotation2d.fromRotations(getCurrentYaw().magnitude())), getModulePositions());
 
         for(int i=0;i<modules.length;i++){
@@ -178,7 +179,7 @@ public class SwerveDriveSystem implements Subsystem{
             MAX_LINEAR_SPEED * x,
             MAX_LINEAR_SPEED * y,
             MAX_ANGULAR_SPEED * rot,
-            Rotation2d.fromDegrees(getCurrentYaw().magnitude())
+            Rotation2d.fromDegrees(getCurrentYaw().in(Units.Degrees))
         ), new Translation2d());
     }
 
@@ -193,7 +194,7 @@ public class SwerveDriveSystem implements Subsystem{
             MAX_LINEAR_SPEED * x,
             MAX_LINEAR_SPEED * y,
             MAX_ANGULAR_SPEED * rot,
-            Rotation2d.fromDegrees(getCurrentYaw().magnitude())
+            Rotation2d.fromDegrees(getCurrentYaw().in(Units.Degrees))
         ), new Translation2d());
     }
 
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java
index 35f1b82..98655ce 100644
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/drivesystems/swerve/SwerveModule.java
@@ -172,7 +172,7 @@ public class SwerveModule {
     }
 
     private double getModuleDistance(){
-        double distance = (this.driveMotor.getPosition(true).getValueAsDouble() * gearRatio) * (Math.PI * wheelDiameterInMeters);
+        double distance = (this.driveMotor.getPosition(true).getValueAsDouble() / gearRatio) * (Math.PI * wheelDiameterInMeters);
         return distance;
     }