Vision

src/main/java/frc/robot/Constants.java

@@ -302,6 +302,16 @@ public static class ClimberConstants { // CAN ID range 25-29
 		}
 	}
 
+	// TODO adjust
+	public static class VisionConstants {
+		public static final double linearStdDevBaseline = 0.02;
+		public static final double angularStdDevBaseline = 0.06;
+
+		public static final double linearStdDevMT2Factor = 0.5;
+		public static final double angularStdDevMT2Factor = Double.POSITIVE_INFINITY;
+
+	}
+
 	public static class OperatorConstants {
 		public static final int kDriverControllerPort = 0;
 		public static final int kAuxControllerPort = 1;

src/main/java/frc/robot/RobotContainer.java

@@ -21,6 +21,10 @@
 import frc.robot.subsystems.swerve.SDSModuleIO;
 import frc.robot.subsystems.swerve.SDSModuleIOSpark;
 import frc.robot.subsystems.swerve.SwerveDrive;
+import frc.robot.subsystems.vision.LimelightLocations;
+import frc.robot.subsystems.vision.Vision;
+import frc.robot.subsystems.vision.VisionIO;
+import frc.robot.subsystems.vision.VisionIOLimelight;
 
 public class RobotContainer {
     private CommandXboxController driverController;
@@ -29,6 +33,7 @@ public class RobotContainer {
     private SwerveDrive swerve;
     private Elevator elevator;
     private Manipulator manipulator;
+    private Vision vision;
 
     public RobotContainer() {
         Preferences.removeAll();
@@ -48,6 +53,12 @@ public RobotContainer() {
                 );
                 // elevator = new Elevator(new ElevatorIOSpark());
                 // manipulator = new Manipulator(new ManipulatorIOSpark());
+                vision = new Vision(
+                    swerve::addVisionMeasurement,
+                    new VisionIOLimelight(LimelightLocations.FRONT3, swerve.getPose()::getRotation),
+                    new VisionIOLimelight(LimelightLocations.HIGH2PLUS, swerve.getPose()::getRotation),
+                    new VisionIOLimelight(LimelightLocations.SIDE2, swerve.getPose()::getRotation)
+                );
                 break;
             default:
                 swerve = new SwerveDrive(
@@ -58,6 +69,7 @@ public RobotContainer() {
                     new SDSModuleIO() {});
                 // elevator = new Elevator(new ElevatorIO() {});
                 // manipulator = new Manipulator(new ManipulatorIO() {});
+                vision = new Vision((pose, timestamp, stdDevs) -> {}, new VisionIO() {});
                 break;
         }
 

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -13,6 +13,7 @@
 import choreo.trajectory.SwerveSample;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
@@ -22,6 +23,8 @@
 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.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Preferences;
@@ -301,6 +304,11 @@ public Pose2d getPose() {
         return poseEstimator.getEstimatedPosition();
     }
 
+    public void addVisionMeasurement(Pose2d visionMeasurement, double timestamp, Matrix<N3,N1> stdDevs) {
+        // higher standard deviations means vision measurements are trusted less
+        poseEstimator.addVisionMeasurement(visionMeasurement, timestamp, stdDevs);
+    }
+
     @Override
     public void periodic() {
         gyroIO.updateInputs(gyroIOInputs);  

src/main/java/frc/robot/subsystems/vision/LimelightLocations.java

@@ -0,0 +1,24 @@
+package frc.robot.subsystems.vision;
+
+public enum LimelightLocations {
+    FRONT3("limelight-front-3", 1, 1),
+    HIGH2PLUS("limelight-high-2+", 1.5, 1.5),
+    SIDE2("limelight-side-2", 2, 2),
+    DEFAULT("limelight", 1, 1)
+    ;
+
+    public final String name;
+    public final double linearStdDevFactor;
+    public final double angularStdDevFactor;
+
+    private LimelightLocations(String name, double linearStdDevFactor, double angularStdDevFactor) {
+        this.name = name;
+        this.linearStdDevFactor = linearStdDevFactor;
+        this.angularStdDevFactor = angularStdDevFactor;
+    }
+
+    @Override
+    public String toString() {
+        return name;
+    }
+}

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

@@ -0,0 +1,89 @@
+package frc.robot.subsystems.vision;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.Alert;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Vision extends SubsystemBase {
+    private PoseEstimateConsumer consumer;
+    private VisionIOWrapper[] wrappedIOs;
+
+    private static final boolean useAtomic = false;
+
+    public Vision(PoseEstimateConsumer consumer, VisionIO... ios) {
+        this.consumer = consumer;
+        wrappedIOs = new VisionIOWrapper[ios.length];
+        for(int i = 0; i < ios.length; i++) {
+            wrappedIOs[i] = new VisionIOWrapper(
+                ios[i],
+                new VisionIOInputsAutoLogged(),
+                new Alert("Vision module " + ios[i].getLimelightLocation().name + " disconnected.", AlertType.kWarning)
+            );
+        }
+    }
+
+    @Override
+    public void periodic() {
+        for(VisionIOWrapper wrappedIO : wrappedIOs) {
+            wrappedIO.io.updateOrientation();
+        }
+        NetworkTableInstance.getDefault().flush(); // TODO check if need; increases network traffic, but is recommended?
+        for(VisionIOWrapper wrappedIO : wrappedIOs) {
+            wrappedIO.io.updateInputs(wrappedIO.inputs);
+            Logger.processInputs("Vision/" + wrappedIO.io.getLimelightLocation().name, wrappedIO.inputs);
+
+            wrappedIO.disconnectedAlert.set(!wrappedIO.inputs.connected);
+
+            if(useAtomic) {
+                consumer.accept(
+                    wrappedIO.inputs.lastPoseObservationMT1.pose().toPose2d(),
+                    wrappedIO.inputs.lastTimestampMT1,
+                    convertStdDevs(wrappedIO.inputs.lastStdDevsMT1)
+                );
+                consumer.accept(
+                    wrappedIO.inputs.lastPoseObservationMT2.pose().toPose2d(),
+                    wrappedIO.inputs.lastTimestampMT2,
+                    convertStdDevs(wrappedIO.inputs.lastStdDevsMT2)
+                );
+            } else {
+                if(wrappedIO.inputs.stdDevsMT1.length + wrappedIO.inputs.stdDevsMT2.length != wrappedIO.inputs.poseObservations.length) {
+                    System.out.println("Expected # of pose observations does not match lengths of standard deviations");
+                }
+                for(int i = 0; i < wrappedIO.inputs.stdDevsMT1.length; i++) {
+                    consumer.accept(
+                        wrappedIO.inputs.poseObservations[i].pose().toPose2d(),
+                        wrappedIO.inputs.poseObservations[i].timestamp(),
+                        convertStdDevs(wrappedIO.inputs.stdDevsMT1[i])
+                    );
+                }
+                for(int i = 0; i < wrappedIO.inputs.stdDevsMT2.length; i++) {
+                    consumer.accept(
+                        wrappedIO.inputs.poseObservations[wrappedIO.inputs.stdDevsMT1.length + i].pose().toPose2d(),
+                        wrappedIO.inputs.poseObservations[wrappedIO.inputs.stdDevsMT1.length + i].timestamp(),
+                        convertStdDevs(wrappedIO.inputs.stdDevsMT2[i])
+                    );
+                }
+            }
+        }
+    }
+
+    private static Matrix<N3, N1> convertStdDevs(double[] stdDevs) {
+        return stdDevs.length == 2 ? VecBuilder.fill(stdDevs[0], stdDevs[0], stdDevs[1]) : VecBuilder.fill(Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY, Double.POSITIVE_INFINITY);
+    }
+
+    @FunctionalInterface
+    public static interface PoseEstimateConsumer {
+        public void accept(Pose2d visionMeasurement, double timestamp, Matrix<N3,N1> stdDevs);
+    }
+
+    private static record VisionIOWrapper(VisionIO io, VisionIOInputsAutoLogged inputs, Alert disconnectedAlert) {}
+}

src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -0,0 +1,52 @@
+package frc.robot.subsystems.vision;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+
+public interface VisionIO {
+    @AutoLog
+    public static class VisionIOInputs {
+        public boolean connected = false;
+        public TargetObservation latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d());
+
+        public double lastTimestampMT1 = 0;
+        public double lastTimestampMT2 = 0;
+        public PoseObservation lastPoseObservationMT1 = new PoseObservation(0, new Pose3d(), 0, 0, 0, PoseObservationType.MEGATAG_1, LimelightLocations.DEFAULT); // get atomic
+        public PoseObservation lastPoseObservationMT2 = new PoseObservation(0, new Pose3d(), 0, 0, 0, PoseObservationType.MEGATAG_2, LimelightLocations.DEFAULT); // get atomic
+        public PoseObservation[] poseObservations = new PoseObservation[0];
+
+        public int[] tagIds = new int[0]; // stores all values in last robot code loop; if desired, add tagIds in just the last observation and/or tagIds per PoseObservation; does not store duplicates
+
+        public double[] lastStdDevsMT1 = new double[2];
+        public double[] lastStdDevsMT2 = new double[2];
+        public double[][] stdDevsMT1 = new double[0][2];
+        public double[][] stdDevsMT2 = new double[0][2];
+    }
+
+    public static record TargetObservation(Rotation2d tx, Rotation2d ty) {}
+
+    public static record PoseObservation(
+      double timestamp,
+      Pose3d pose,
+      double ambiguity,
+      int tagCount,
+      double averageTagDistance,
+      PoseObservationType type,
+      LimelightLocations limelightLocation
+    ) {}
+
+    public static enum PoseObservationType {
+        MEGATAG_1,
+        MEGATAG_2
+    }
+
+    public default LimelightLocations getLimelightLocation() {
+        return LimelightLocations.DEFAULT;
+    }
+
+    public default void updateOrientation() {}
+
+    public default void updateInputs(VisionIOInputs inputs) {}
+}