From 89c1914d11b765bcedcd008974085f2f0c8206b2 Mon Sep 17 00:00:00 2001
From: Tylr-J42 <Tyler.Jacques00@gmail.com>
Date: Thu, 30 Jan 2025 04:14:57 -0500
Subject: [PATCH] drivetrain odometry -> pose estimator

---
 .../java/frc/robot/subsystems/Drivetrain.java | 27 +++++++++----------
 .../{TylerVision.java => Vision.java}         |  4 +--
 2 files changed, 15 insertions(+), 16 deletions(-)
 rename src/main/java/frc/robot/subsystems/{TylerVision.java => Vision.java} (96%)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 0e4689e..18972a0 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -13,11 +13,11 @@ import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -38,7 +38,7 @@ public class Drivetrain extends SubsystemBase {
     private AHRS ahrs;
 
     // Odometry class for tracking robot pose
-    private SwerveDriveOdometry m_odometry;
+    private SwerveDrivePoseEstimator m_estimator;
 
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
@@ -68,7 +68,7 @@ public class Drivetrain extends SubsystemBase {
 
         ahrs = new AHRS(NavXComType.kMXP_SPI);
 
-        m_odometry = new SwerveDriveOdometry(
+        m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
             Rotation2d.fromDegrees(ahrs.getAngle()),
             new SwerveModulePosition[] {
@@ -76,7 +76,9 @@ public class Drivetrain extends SubsystemBase {
                 m_frontRight.getPosition(),
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
-        });
+            },
+            new Pose2d()
+        );
 
         AutoBuilder.configure(
             this::getPose, 
@@ -99,7 +101,7 @@ public class Drivetrain extends SubsystemBase {
     @Override
     public void periodic() {
         // Update the odometry in the periodic block
-        m_odometry.update(
+        m_estimator.update(
             Rotation2d.fromDegrees(getGyroValue()),
             new SwerveModulePosition[] {
                 m_frontLeft.getPosition(),
@@ -132,7 +134,7 @@ public class Drivetrain extends SubsystemBase {
      * @return The pose.
      */
     public Pose2d getPose() {
-        return m_odometry.getPoseMeters();
+        return m_estimator.getEstimatedPosition();
     }
 
     /**
@@ -141,14 +143,7 @@ public class Drivetrain extends SubsystemBase {
      * @param pose The pose to which to set the odometry.
      */
     public void resetOdometry(Pose2d pose) {
-        m_odometry.resetPosition(
-            Rotation2d.fromDegrees(getGyroValue()),
-            new SwerveModulePosition[] {
-                m_frontLeft.getPosition(),
-                m_frontRight.getPosition(),
-                m_rearLeft.getPosition(),
-                m_rearRight.getPosition()
-            },
+        m_estimator.resetPose(
             pose
         );
     }
@@ -257,4 +252,8 @@ public class Drivetrain extends SubsystemBase {
     public double getTurnRate() {
         return ahrs.getRate() * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
     }
+
+    public void addVisionMeasurement(Pose2d pose, double timestamp){
+        m_estimator.addVisionMeasurement(pose, timestamp);
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/TylerVision.java b/src/main/java/frc/robot/subsystems/Vision.java
similarity index 96%
rename from src/main/java/frc/robot/subsystems/TylerVision.java
rename to src/main/java/frc/robot/subsystems/Vision.java
index 271fd11..72a16a0 100644
--- a/src/main/java/frc/robot/subsystems/TylerVision.java
+++ b/src/main/java/frc/robot/subsystems/Vision.java
@@ -5,7 +5,7 @@ import edu.wpi.first.networktables.DoubleSubscriber;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 
-public class TylerVision{
+public class Vision{
 
     private DoubleSubscriber cam1GlobalPose;
     private DoubleSubscriber cam1ClosestTag;
@@ -13,7 +13,7 @@ public class TylerVision{
 
     private DoubleSubscriber framerate;
 
-    public TylerVision(){
+    public Vision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
         NetworkTable visionTable = inst.getTable("Fiducial");