visionOdo

src/main/java/frc4388/robot/subsystems/SwerveDrive.java

@@ -61,7 +61,6 @@ public class SwerveDrive extends SubsystemBase {
    */
   public SwerveDrivePoseEstimator m_poseEstimator;
   public SwerveDriveOdometry m_odometry;
-  public VisionOdometry m_visionOdometry;
 
   public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
   public boolean ignoreAngles;
@@ -249,16 +248,8 @@ public class SwerveDrive extends SubsystemBase {
                             modules[1].getState(), 
                             modules[2].getState(), 
                             modules[3].getState());
+  }
   
-      // Also apply vision measurements if the camera can get vision
-      try {
-        m_poseEstimator.addVisionMeasurement(
-                m_visionOdometry.getVisionOdometry(),
-                Timer.getFPGATimestamp() - m_visionOdometry.getLatency());
-      } catch (VisionObscuredException err) {
-        err.printStackTrace();
-      }
-    }
   
   /**
    * Resets pigeon.

src/main/java/frc4388/robot/subsystems/VisionOdometry.java

@@ -20,6 +20,7 @@ import org.photonvision.targeting.TargetCorner;
 
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.VisionConstants;
@@ -124,7 +125,20 @@ public class VisionOdometry extends SubsystemBase {
   public double getLatency() {
     return latency;
   }
+  public boolean getLEDs() {
+    if (m_camera.getLEDMode() == VisionLEDMode.kOff) return false;
+    return true;
+  }
 
+  public void updateOdometryWithVision(){
+    try {
+      m_drive.m_poseEstimator.addVisionMeasurement(
+              getVisionOdometry(),
+              Timer.getFPGATimestamp() - getLatency());
+    } catch (VisionObscuredException err) {
+      err.printStackTrace();
+    } 
+  }
   /** Reverse 3d projects target points from screen coordinates to 3d space
    * <p>
    * Uses the known height of the target to project points