From 649660ade69bb360227b08a91ce739aad8ae4d70 Mon Sep 17 00:00:00 2001
From: Team 2648 <team2648@coldlightalchemist.com>
Date: Fri, 7 Mar 2025 19:04:53 -0500
Subject: [PATCH] trying to fuse apriltag pose with odometry

---
 src/main/java/frc/robot/subsystems/Drivetrain.java | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 661b977..4fdca3f 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -17,6 +17,7 @@ import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
@@ -26,6 +27,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.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.Filesystem;
@@ -104,6 +106,8 @@ public class Drivetrain extends SubsystemBase {
             new Pose2d()
         );
 
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
+
         pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
         pidHeading.enableContinuousInput(-180, 180);