diff --git a/src/main/java/frc/robot/subsystems/Drive.java b/src/main/java/frc/robot/subsystems/Drive.java index 6dcbd6b..a845361 100644 --- 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()); } }