From 0e49f32246cf7a717fe8ca66d29f5621e18ed6a4 Mon Sep 17 00:00:00 2001 From: moo Date: Wed, 13 May 2026 22:13:05 -0700 Subject: [PATCH] iteration #1 --- .../subsystems/drivetrain/Drivetrain.java | 20 ++++++ .../java/frc/robot/util/Vision/Vision.java | 70 ++++++++++++++++++- 2 files changed, 89 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 0db0422..022d0b2 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -701,6 +701,26 @@ public class Drivetrain extends SubsystemBase { } } + /** + * Sets weight for an AprilTag + * 1.0 is default, >1 trusts more, 0-1 trusts less, <=0 disables + * + * @param id AprilTag id + * @param weight weight to apply for this tag + */ + public void setAprilTagWeight(int id, double weight) { + if (vision != null) { + vision.setTagWeight(id, weight); + } + } + + /** Clears all custom AprilTag weights */ + public void clearAprilTagWeights() { + if (vision != null) { + vision.clearTagWeights(); + } + } + /** * Returns if vision has seen an April tag in the last frame * diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java index d17bdd2..d5f7c33 100644 --- a/src/main/java/frc/robot/util/Vision/Vision.java +++ b/src/main/java/frc/robot/util/Vision/Vision.java @@ -1,6 +1,7 @@ package frc.robot.util.Vision; import java.util.ArrayList; +import java.util.Arrays; import java.util.List; import java.util.Optional; import java.util.function.DoubleUnaryOperator; @@ -16,6 +17,7 @@ import org.photonvision.targeting.PhotonTrackedTarget; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFieldLayout.OriginPosition; +import edu.wpi.first.math.Matrix; import edu.wpi.first.math.Pair; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -25,6 +27,8 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; @@ -58,6 +62,10 @@ public class Vision { // Array of tags to use, null or empty array to use all tags private int[] onlyUse = null; + // Index by tag ID, index 0 is unused. + // 1.0 is default, >1 trusts more, 0-1 trusts less, <=0 disables + private final double[] tagWeights; + /** * Creates a new instance of Vision and sets up the cameras and field layout */ @@ -77,6 +85,8 @@ public class Vision { // Sets the origin to the right side of the blue alliance wall FieldConstants.field.setOrigin(OriginPosition.kBlueAllianceWallRightSide); + tagWeights = new double[FieldConstants.field.getTags().size() + 1]; + Arrays.fill(tagWeights, 1.0); if(VisionConstants.ENABLED){ // Puts the cameras in an array list @@ -363,13 +373,38 @@ public class Vision { poseEstimator.addVisionMeasurement( estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds, - slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS + getVisionStdDevsForPose(estimatedPose, slipped) ); sawTag = true; } return estimatedPoses; } + private Matrix getVisionStdDevsForPose(EstimatedRobotPose estimatedPose, boolean slipped) { + Matrix baseStdDevs = slipped ? VisionConstants.VISION_STD_DEVS_2 : VisionConstants.VISION_STD_DEVS; + + if (estimatedPose == null || estimatedPose.targetsUsed == null || estimatedPose.targetsUsed.isEmpty()) { + return baseStdDevs; + } + + double weightSum = 0.0; + int weightCount = 0; + for (PhotonTrackedTarget target : estimatedPose.targetsUsed) { + double weight = getTagWeight(target.getFiducialId()); + if (weight > 0.0) { + weightSum += weight; + weightCount++; + } + } + + if (weightCount == 0) { + return baseStdDevs.times(1e6); + } + + double averageWeight = weightSum / weightCount; + return baseStdDevs.times(1.0 / averageWeight); + } + /** * Updates each camera's inputs for logging */ @@ -407,6 +442,36 @@ public class Vision { onlyUse = ids; } + /** + * Sets the weighting for an AprilTag + * @param id AprilTag id + * @param weight Tag weight (1.0 = default, >1 more trust, 0-1 less trust, <=0 disable) + */ + public void setTagWeight(int id, double weight) { + if (id <= 0 || id >= tagWeights.length) { + DriverStation.reportWarning("Vision.setTagWeight: AprilTag id " + id + " is out of bounds", false); //we should use these more often + return; + } + tagWeights[id] = weight; + } + + /** + * Gets the weight of an AprilTag + * @param id AprilTag id + * @return Tag weight (default 1 if out of range) + */ + public double getTagWeight(int id) { + if (id <= 0 || id >= tagWeights.length) { + return 1.0; + } + return tagWeights[id]; + } + + /** Clears all custom AprilTag weights */ + public void clearTagWeights() { + Arrays.fill(tagWeights, 1.0); + } + /** * Checks if one or more cameras are disconnected * @return true if at least one camera is disconnected, false otherwise @@ -693,6 +758,9 @@ public class Vision { if(id <= 0 || id > FieldConstants.field.getTags().size()){ return false; } + if(getTagWeight(id) <= 0){ + return false; + } // Return false if it is in the list of tags to ignore for(int id2 : VisionConstants.TAGS_TO_IGNORE){ if(id == id2){ -- 2.39.5