fixed error where vision was not checking ambiguity tolerances when u…

…sing multitag
blair-robot-project · Oct 27, 2024 · ce13edb · ce13edb
1 parent 8b363fe
commit ce13edb
Showing 1 changed file with 8 additions and 4 deletions.
diff --git a/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt b/src/main/kotlin/frc/team449/control/holonomic/SwerveDrive.kt
@@ -265,11 +265,15 @@ open class SwerveDrive(
         visionPose[1 + 3 * index] = estVisionPose.y
         visionPose[2 + 3 * index] = estVisionPose.rotation.radians
 
+        val inAmbiguityTolerance = avgAmbiguity[index] <= VisionConstants.MAX_AMBIGUITY
+        val inDistanceTolerance = numTargets[index] < 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_SINGLE_TAG ||
+          numTargets[index] >= 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_MULTI_TAG + (numTargets[index] - 2) * VisionConstants.NUM_TAG_FACTOR
+        val inHeightTolerance = heightError[index] < VisionConstants.MAX_HEIGHT_ERR_METERS
+
         if (presentResult.timestampSeconds > 0 &&
-          avgAmbiguity[index] <= VisionConstants.MAX_AMBIGUITY &&
-          numTargets[index] < 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_SINGLE_TAG ||
-          numTargets[index] >= 2 && tagDistance[index] <= VisionConstants.MAX_DISTANCE_MULTI_TAG + (numTargets[index] - 2) * VisionConstants.NUM_TAG_FACTOR &&
-          heightError[index] < VisionConstants.MAX_HEIGHT_ERR_METERS
+          inAmbiguityTolerance &&
+          inDistanceTolerance &&
+          inHeightTolerance
         ) {
           if (enableVisionFusion) {
             poseEstimator.addVisionMeasurement(