ok idk why it says atticus did all the things but THIS GUY did it!!!!! instantiated vision (if i did it right)!!!!!!!!!!!!

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

@@ -35,6 +35,8 @@
 import com.ctre.phoenix6.signals.RGBWColor;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.system.plant.DCMotor;
@@ -55,6 +57,9 @@
 import frc.lib.W8.util.Device;
 import frc.lib.W8.util.Device.CAN;
 import frc.lib.W8.util.MechanismUtil.DistanceAngleConverter;
+import java.util.Arrays;
+import java.util.List;
+import org.photonvision.simulation.VisionSystemSim;
 
 /**
  * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
@@ -457,6 +462,50 @@ public static CANcoderConfiguration getCANcoderConfig(boolean sim) {
       return config;
     }
 
+    public class VisionConstants {
+      // AprilTag layout
+      public static AprilTagFieldLayout aprilTagLayout =
+          AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);
+
+      // Camera names, must match names configured on coprocessor
+      public static String camera0Name = "camera_0";
+      public static String camera1Name = "camera_1";
+
+      // Robot to camera transforms
+      // (Not used by Limelight, configure in web UI instead)
+      public static Transform3d robotToCamera0 =
+          new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0));
+      public static Transform3d robotToCamera1 =
+          new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI));
+
+      // Basic filtering thresholds
+      public static double maxAmbiguity = 0.3;
+      public static double maxZError = 0.75;
+
+      // Standard deviation baselines, for 1 meter distance and 1 tag
+      // (Adjusted automatically based on distance and # of tags)
+      public static double linearStdDevBaseline = 0.02; // Meters
+      public static double angularStdDevBaseline = 0.06; // Radians
+
+      // Standard deviation multipliers for each camera
+      // (Adjust to trust some cameras more than others)
+      public static double[] cameraStdDevFactors =
+          new double[] {
+            1.0, // Camera 0
+            1.0 // Camera 1
+          };
+
+      /** Tags used for reef alignment */
+      public static List<Integer> alignmentTags =
+          Arrays.asList(6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22);
+
+      public static VisionSystemSim getSystemSim() {
+        var system = new VisionSystemSim("main");
+        system.addAprilTags(aprilTagLayout);
+        return system;
+      }
+    }
+
     public static TalonFXConfiguration getFXConfig() {
       TalonFXConfiguration config = new TalonFXConfiguration();
 

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

@@ -24,6 +24,8 @@
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.lib.W8.io.absoluteencoder.AbsoluteEncoderIOCANCoderSim;
 import frc.lib.W8.io.motor.*;
+import frc.lib.W8.io.vision.VisionIOPhotonVision;
+import frc.lib.W8.io.vision.VisionIOPhotonVisionSim;
 import frc.lib.W8.mechanisms.flywheel.*;
 import frc.lib.W8.mechanisms.linear.LinearMechanism;
 import frc.lib.W8.mechanisms.linear.LinearMechanismReal;
@@ -34,6 +36,7 @@
 import frc.robot.Constants.ClimberConstants;
 import frc.robot.Constants.HopperConstants;
 import frc.robot.Constants.IntakeConstants;
+import frc.robot.Constants.IntakeConstants.VisionConstants;
 import frc.robot.Constants.Ports;
 import frc.robot.Constants.ShooterFlywheelConstants;
 import frc.robot.Constants.ShooterRotaryConstants;
@@ -43,6 +46,7 @@
 import frc.robot.subsystems.Hopper;
 import frc.robot.subsystems.Intake;
 import frc.robot.subsystems.Shooter;
+import frc.robot.subsystems.Vision;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.GyroIO;
 import frc.robot.subsystems.drive.GyroIOPigeon2;
@@ -51,6 +55,7 @@
 import frc.robot.subsystems.drive.ModuleIOTalonFX;
 import java.util.Optional;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -65,6 +70,7 @@ public class RobotContainer {
   private final Shooter shooter;
   private final Climber climber;
   private final Intake intake;
+  private final Vision vision;
 
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
@@ -137,6 +143,13 @@ public RobotContainer() {
                             Ports.IntakeRoller,
                             IntakeConstants.MOTOR_NAME + " Encoder",
                             IntakeConstants.getCANcoderConfig(false)))));
+        vision =
+            new Vision(
+                new VisionIOPhotonVision(
+                    VisionConstants.camera0Name,
+                    VisionConstants.robotToCamera0,
+                    VisionConstants.aprilTagLayout,
+                    PoseStrategy.CONSTRAINED_SOLVEPNP));
         break;
 
       case SIM:
@@ -223,6 +236,15 @@ public RobotContainer() {
                             Ports.IntakeRoller,
                             IntakeConstants.MOTOR_NAME + " Encoder",
                             IntakeConstants.getCANcoderConfig(false)))));
+        vision =
+            new Vision(
+                new VisionIOPhotonVisionSim(
+                    () -> drive.getPose(),
+                    VisionConstants.camera0Name,
+                    VisionConstants.robotToCamera0,
+                    VisionConstants.aprilTagLayout,
+                    PoseStrategy.CONSTRAINED_SOLVEPNP,
+                    VisionConstants.getSystemSim()));
         break;
 
       default:
@@ -251,6 +273,7 @@ public RobotContainer() {
             new Intake(
                 new FlywheelMechanism() {},
                 new RotaryMechanism(IntakeConstants.MOTOR_NAME, IntakeConstants.CONSTANTS) {});
+        vision = new Vision(null);
         break;
     }