diff --git a/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java b/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java index 7ea93a1..1ff1b5b 100644 --- a/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java +++ b/src/main/java/frc/robot/subsystems/vision/TurretCameraVisionIO.java @@ -102,8 +102,8 @@ private void handleApriltags( double tagDist = results.get().targetDistance; double ta = results.get().ta; Logger.recordOutput("Limelight/Gabriel/tagDist", tagDist); - stdDevX *= Math.pow(tagDist, alpha) * Math.pow(1.0 / ta, beta); - stdDevY *= Math.pow(tagDist, alpha) * Math.pow(1.0 / ta, beta); + stdDevX *= Math.pow(tagDist, alpha) * Math.pow(1.0 / (ta + 1e-4), beta); + stdDevY *= Math.pow(tagDist, alpha) * Math.pow(1.0 / (ta + 1e-4), beta); stdDevYaw *= tagDist; } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java index e015b87..c7c6b22 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java @@ -76,8 +76,8 @@ private void handleApriltags(VisionInputs generalInputs, ApriltagInputs apriltag double tagDist = results.get().targetDistance; double ta = results.get().ta; Logger.recordOutput("Limelight/Gabriel/tagDist", tagDist); - stdDevX *= Math.pow(tagDist, alpha) * Math.pow(1.0 / ta, beta); - stdDevY *= Math.pow(tagDist, alpha) * Math.pow(1.0 / ta, beta); + stdDevX *= Math.pow(tagDist, alpha) * Math.pow(1.0 / (ta + 1e-4), beta); + stdDevY *= Math.pow(tagDist, alpha) * Math.pow(1.0 / (ta + 1e-4), beta); stdDevYaw *= tagDist; }