Started adding the vision subsystem

Author: Huck-Richardson

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

@@ -24,6 +24,7 @@ public final class VisionConstants {
   //   public static final double kThetaStdDevRejected = Units.degreesToRadians(360);
   //   public static final double kThetaStdThres = 0.2;
 
+
   // Velocity Filter
   public static final double kLinearCoeffOnVelFilter = 0.1;
   public static final double kOffsetOnVelFilter = 0.10;

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

@@ -1,3 +1,49 @@
 package frc.robot.subsystems.vision;
 
-public class VisionSubsystem {}
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc.robot.constants.VisionConstants;
+import java.util.Set;
+import org.strykeforce.telemetry.measurable.MeasurableSubsystem;
+import org.strykeforce.telemetry.measurable.Measure;
+import frc.robot.subsystems.drive.DriveSubsystem;
+
+public class VisionSubsystem extends MeasurableSubsystem {
+
+  Translation2d[] camPositions = {
+    VisionConstants.kCam1Pose.getTranslation().toTranslation2d(),
+    VisionConstants.kCam2Pose.getTranslation().toTranslation2d(),
+    VisionConstants.kCam3Pose.getTranslation().toTranslation2d(),
+    VisionConstants.kCam4Pose.getTranslation().toTranslation2d()
+  };
+
+  Rotation2d[] camRotations = {
+    VisionConstants.kCam1Pose.getRotation().toRotation2d(),
+    VisionConstants.kCam2Pose.getRotation().toRotation2d(),
+    VisionConstants.kCam3Pose.getRotation().toRotation2d(),
+    VisionConstants.kCam4Pose.getRotation().toRotation2d()
+  };
+
+  String[] camNames = {
+    VisionConstants.kCam1Name, VisionConstants.kCam2Name, VisionConstants.kCam3Name
+  };
+
+  String[] piNames = {
+    VisionConstants.kPi1Name, VisionConstants.kPi2Name, VisionConstants.kPi3Name,
+  };
+
+  int[] camIndex = {
+    VisionConstants.kCam1Idx,
+    VisionConstants.kCam2Idx,
+    VisionConstants.kCam3Idx,
+    VisionConstants.kCam4Idx
+  };
+
+  public VisionSubsystem(DriveSubsystem driveSubsystem) {
+
+  }
+  @Override
+  public Set<Measure> getMeasures() {
+    return Set.of();
+  }
+}