Updated vision code for 2022

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

@@ -17,12 +17,14 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.OIConstants;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+import frc4388.utility.VisionObscuredException;
 
 public class SwerveDrive extends SubsystemBase {
 
@@ -51,6 +53,7 @@ public class SwerveDrive extends SubsystemBase {
   below are robot specific, and should be tuned. */
   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;
@@ -227,11 +230,14 @@ public class SwerveDrive extends SubsystemBase {
                             modules[2].getState(), 
                             modules[3].getState());
   
-      // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
-      // a real robot, this must be calculated based either on latency or timestamps.
-      // m_poseEstimator.addVisionMeasurement(
-      //         m_poseEstimator.getEstimatedPosition(),
-      //     Timer.getFPGATimestamp() - 0.1);
+      // 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();
+      }
     }
   
   /**