From 49399c717270ee3ffd7bd94bce7fcbeceebc2cf2 Mon Sep 17 00:00:00 2001
From: TheRealGrayden <nmunier65@gmail.com>
Date: Sat, 21 Mar 2026 14:23:19 -0400
Subject: [PATCH] Lights

---
 .../robolib/vision/limelight/Limelight.java   | 32 +++++++++++++
 .../vision/limelight/LimelightSubsystem.java  | 47 -------------------
 2 files changed, 32 insertions(+), 47 deletions(-)
 create mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java
 delete mode 100644 lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/LimelightSubsystem.java

diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java
new file mode 100644
index 0000000..6ad1cf4
--- /dev/null
+++ b/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/Limelight.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.fairportrobotics.frc.robolib.vision.limelight;
+
+import com.ctre.phoenix6.hardware.Pigeon2;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator3d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+
+public class Limelight {
+  private String name;
+  private Pigeon2 gyro;
+  private SwerveDrivePoseEstimator3d poseEstimator;
+
+  public Limelight(String name, Pigeon2 gyro, SwerveDrivePoseEstimator3d poseEstimator) {
+    this.name = name;
+    this.gyro = gyro;
+    this.poseEstimator = poseEstimator;
+  } 
+
+  public LimelightHelpers.PoseEstimate updateOdometry() {
+      boolean useMegaTag2 = true; //set to false to use MegaTag1
+      boolean doRejectUpdate = false;
+      
+      LimelightHelpers.SetRobotOrientation(name, Math.toDegrees(poseEstimator.getEstimatedPosition().getRotation().getZ()), 0, 0, 0, 0, 0);
+      return LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight");
+    }
+}
diff --git a/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/LimelightSubsystem.java b/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/LimelightSubsystem.java
deleted file mode 100644
index 5eb79c2..0000000
--- a/lib/src/main/java/org/fairportrobotics/frc/robolib/vision/limelight/LimelightSubsystem.java
+++ /dev/null
@@ -1,47 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package org.fairportrobotics.frc.robolib.vision.limelight;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-public class LimelightSubsystem extends SubsystemBase {
-  /** Creates a new ExampleSubsystem. */
-  public LimelightSubsystem() {}
-
-  /**
-   * Example command factory method.
-   *
-   * @return a command
-   */
-  public Command exampleMethodCommand() {
-    // Inline construction of command goes here.
-    // Subsystem::RunOnce implicitly requires `this` subsystem.
-    return runOnce(
-        () -> {
-          /* one-time action goes here */
-        });
-  }
-
-  /**
-   * An example method querying a boolean state of the subsystem (for example, a digital sensor).
-   *
-   * @return value of some boolean subsystem state, such as a digital sensor.
-   */
-  public boolean exampleCondition() {
-    // Query some boolean state, such as a digital sensor.
-    return false;
-  }
-
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
-  }
-
-  @Override
-  public void simulationPeriodic() {
-    // This method will be called once per scheduler run during simulation
-  }
-}