adjusting vision and odometry fusion

src/main/java/frc/robot/constants/VisionConstants.java

@@ -59,6 +59,6 @@ public class VisionConstants {
         {4.993, 2.816, 5.272, 2.996}
     };
 
-    public static final double latencyFudge = 0.080;
+    public static final double latencyFudge = 0;
 
 }

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -66,6 +66,7 @@ public class Drivetrain extends SubsystemBase {
     public Vision vision;
 
     public Pose2d orangePose2d;
+    public Pose2d blackPose2d;
 
     /** Creates a new DriveSubsystem. */
     public Drivetrain() {
@@ -94,6 +95,7 @@ public class Drivetrain extends SubsystemBase {
         );
 
         gyro = new AHRS(NavXComType.kMXP_SPI);
+        gyro.reset();
 
         m_estimator = new SwerveDrivePoseEstimator(
             DrivetrainConstants.kDriveKinematics,
@@ -104,11 +106,11 @@ public class Drivetrain extends SubsystemBase {
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
             },
-            new Pose2d()
+            new Pose2d(),
+            VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+            VecBuilder.fill(1, 1, Units.degreesToRadians(360))
         );
 
-        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
-
         pidHeading = new PIDController(DrivetrainConstants.kHeadingP,0,0);
         pidHeading.enableContinuousInput(-180, 180);
 
@@ -152,6 +154,7 @@ public class Drivetrain extends SubsystemBase {
 
         vision = new Vision();
         orangePose2d = new Pose2d();
+        blackPose2d = new Pose2d();
     }
 
     @Override
@@ -168,12 +171,13 @@ public class Drivetrain extends SubsystemBase {
 
         gyroBuffer.addSample(Timer.getFPGATimestamp(), getGyroValue());
 
-       // if(vision.getOrangeDist() < 72){
+        m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(1, 1, Units.degreesToRadians(360)));
-         //   m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.5, 0.5, 0.9));
-        //}
 
-        /* 
         if(vision.getOrangeTagDetected()){
+            if(vision.getOrangeDist() < 60){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
+            }
+
             // if the detected tags match your alliances reef tags use their pose estimates  
             if(vision.getOrangeClosestTag() >= 6 && vision.getOrangeClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getOrangeTagDetected()){
                 orangePose2d = vision.getOrangeGlobalPose(gyroBuffer);
@@ -184,26 +188,24 @@ public class Drivetrain extends SubsystemBase {
                 m_estimator.addVisionMeasurement(orangePose2d, vision.getOrangeTimeStamp());
             }
             Logger.recordOutput("orange pose", new Pose3d());
-            if(orangePose2d.equals(null)){
-                Logger.recordOutput("orange pose", new Pose3d());
-            }else{
-                Logger.recordOutput("orange pose", new Pose3d(orangePose2d));
         }
 
-            Logger.recordOutput("fused robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
+        if(vision.getBlackTagDetected()){
+            if(vision.getBlackDist() < 60){
+                m_estimator.setVisionMeasurementStdDevs(VecBuilder.fill(0.4, 0.4, Units.degreesToRadians(360)));
             }
 
             if(vision.getBlackClosestTag() >= 6 && vision.getBlackClosestTag() <= 11 && DriverStation.getAlliance().get().equals(Alliance.Red) && vision.getBlackTagDetected()){
-            Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
-            Logger.recordOutput("black pose", new Pose3d(blackPose));
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp()); 
-            m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp()); 
 
             }else if(vision.getBlackClosestTag() >= 17 && vision.getBlackClosestTag() <= 22 && DriverStation.getAlliance().get().equals(Alliance.Blue) && vision.getBlackTagDetected()){
-            Pose2d blackPose = vision.getBlackGlobalPose(gyroBuffer);
+                blackPose2d = vision.getBlackGlobalPose(gyroBuffer);
-            Logger.recordOutput("black pose", new Pose3d(blackPose));
+                m_estimator.addVisionMeasurement(blackPose2d, vision.getBlackTimeStamp());
-            m_estimator.addVisionMeasurement(blackPose, vision.getBlackTimeStamp());
             }
-            */
+            Logger.recordOutput("black pose", new Pose3d(blackPose2d));
+        }
+        
         
         Logger.recordOutput("robot pose", new Pose3d(m_estimator.getEstimatedPosition()));
         

src/main/java/frc/robot/subsystems/Vision.java

@@ -15,6 +15,8 @@ import frc.robot.constants.VisionConstants;
 
 public class Vision{
 
+    private NetworkTable blackVisionTable;
+
     private DoubleSubscriber black_tx;
     private DoubleSubscriber black_ty;
     private DoubleSubscriber black_dist;
@@ -24,6 +26,8 @@ public class Vision{
 
     private DoubleSubscriber blackFramerate;
 
+    private NetworkTable orangeVisionTable;
+
     private DoubleSubscriber orange_tx;
     private DoubleSubscriber orange_ty;
     private DoubleSubscriber orange_dist;
@@ -39,8 +43,8 @@ public class Vision{
     public Vision(){
         NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
-        NetworkTable blackVisionTable = inst.getTable("black_Fiducial");
+        blackVisionTable = inst.getTable("black_Fiducial");
-        NetworkTable orangeVisionTable = inst.getTable("orange_Fiducial");
+        orangeVisionTable = inst.getTable("orange_Fiducial");
 
         black_tx = blackVisionTable.getDoubleTopic("tx").subscribe(0.0);
         black_ty = blackVisionTable.getDoubleTopic("ty").subscribe(0.0);
@@ -103,7 +107,7 @@ public class Vision{
 
     public Pose2d getBlackGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
         return cameraToGlobalPose2d(getBlackClosestTag(), orange_dist.get(),
-            getBlackTX(), getBlackTY(), getOrangeTimeStamp(), gyroBuffer, blackCamPose);
+            getBlackTX(), getBlackTY(), getBlackTimeStamp(), gyroBuffer, blackCamPose);
     }
 
     public double getBlackTX(){
@@ -123,7 +127,7 @@ public class Vision{
     }
 
     public double getBlackTimeStamp(){
-        return black_tx.getLastChange();
+        return black_tx.getLastChange()-VisionConstants.latencyFudge;
     }
 
     public boolean getBlackTagDetected(){
@@ -137,7 +141,7 @@ public class Vision{
     public Pose2d getOrangeGlobalPose(TimeInterpolatableBuffer<Double> gyroBuffer){
         if(getOrangeClosestTag() >= 1 && getOrangeClosestTag() <= 22){
             return cameraToGlobalPose2d(getOrangeClosestTag(), orange_dist.get(),
-                orange_tx.get(), orange_ty.get(), Timer.getTimestamp(), gyroBuffer, orangeCamPose
+                orange_tx.get(), orange_ty.get(), getOrangeTimeStamp(), gyroBuffer, orangeCamPose
                 );
         }else{
             return new Pose2d();