implement limelightManager into Drive

--Gabe
frc937 · Oct 23, 2023 · 2cf7a1b · 2cf7a1b
1 parent 1534399
commit 2cf7a1b
Showing 1 changed file with 5 additions and 4 deletions.
diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java
@@ -349,17 +349,18 @@ public void periodic() {
         gyroscope.getRotation2d(),
         this.getAverageLeftPosition(),
         this.getAverageRightPosition());
-    if (limelightManager.hasValidTarget()) {
+    Limelight limelight = limelightManager.getTargetedLimelight();
+    if (limelight != null) {
       /* This *should* check if the pose from the limelight is within 1m of the current odometry pose,
        * which the odometry recommends we do to prevent us from getting noisy measurements
        */
       if ((Math.abs(
-                  limelightManager.getBotpose2d().getX() - whereTheHeckAreWe.getEstimatedPosition().getX())
+                  limelight.getBotpose2d().getX() - whereTheHeckAreWe.getEstimatedPosition().getX())
               >= 1)
           && (Math.abs(
-                  limelightManager.getBotpose2d().getY() - whereTheHeckAreWe.getEstimatedPosition().getY())
+                  limelight.getBotpose2d().getY() - whereTheHeckAreWe.getEstimatedPosition().getY())
               >= 1)) {
-        whereTheHeckAreWe.addVisionMeasurement(limelightManager.getBotpose2d(), Timer.getFPGATimestamp());
+        whereTheHeckAreWe.addVisionMeasurement(limelight.getBotpose2d(), Timer.getFPGATimestamp());
       }
     }