From a063cb18dc28b0ba7199c52911e86c54e90c3bf3 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 12 Jul 2025 16:00:32 -0700 Subject: [PATCH 01/43] first commit of vibecoded vision revamp --- src/main/java/frc/robot/Constants.java | 30 ++ .../frc/robot/subsystems/VisionSystem.java | 419 +++++++++--------- 2 files changed, 236 insertions(+), 213 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ef43561..477d98c8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -14,7 +14,10 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +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; @@ -90,6 +93,33 @@ public String getLoggingName() { public static final double LEFT_CAM_TO_ROBOT_ROTATION_ROLL = 0; public static final double LEFT_CAM_TO_ROBOT_ROTATION_PITCH = Units.degreesToRadians(-12.5); public static final double LEFT_CAM_TO_ROBOT_ROTATION_YAW = Units.degreesToRadians(-40); + + public static Transform3d getCameraTransform(Cameras cam) { + switch (cam) { + case RIGHT_CAM: + return new Transform3d( + new Translation3d( + RIGHT_CAM_TO_ROBOT_TRANSLATION_X, + RIGHT_CAM_TO_ROBOT_TRANSLATION_Y, + RIGHT_CAM_TO_ROBOT_TRANSLATION_Z), + new Rotation3d( + RIGHT_CAM_TO_ROBOT_ROTATION_ROLL, + RIGHT_CAM_TO_ROBOT_ROTATION_PITCH, + RIGHT_CAM_TO_ROBOT_ROTATION_YAW)); + case LEFT_CAM: + return new Transform3d( + new Translation3d( + LEFT_CAM_TO_ROBOT_TRANSLATION_X, + LEFT_CAM_TO_ROBOT_TRANSLATION_Y, + LEFT_CAM_TO_ROBOT_TRANSLATION_Z), + new Rotation3d( + LEFT_CAM_TO_ROBOT_ROTATION_ROLL, + LEFT_CAM_TO_ROBOT_ROTATION_PITCH, + LEFT_CAM_TO_ROBOT_ROTATION_YAW)); + default: + throw new IllegalArgumentException("Unknown camera: " + cam); + } + } } public static class AutoRoutines { diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 5004edc8..3d786484 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -11,21 +11,17 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -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.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.util.MiscUtils; -import java.util.ArrayList; -import java.util.Arrays; import java.util.List; import java.util.Optional; import java.util.function.BooleanSupplier; +import java.util.stream.Collectors; import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; @@ -33,242 +29,239 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; -/** Creates a new VisionSystem. */ +/** + * VisionSystem fuses multi-tag AprilTag measurements from PhotonVision with swerve odometry by + * dynamically computing measurement noise based on tag count, distance, viewing angle, and robot + * speed. + */ public class VisionSystem extends SubsystemBase { - - public static int numThrowaways = 0; - public static int numNotThrowaways = 0; - List reefIDs = - new ArrayList(Arrays.asList(19, 20, 21, 22, 17, 18, 6, 7, 8, 9, 10, 11)); - List blueReefID = new ArrayList(Arrays.asList(19, 20, 21, 22, 17, 18)); - List redReefID = new ArrayList(Arrays.asList(6, 7, 8, 9, 10, 11)); - - Pose2d savedResult = new Pose2d(0, 0, new Rotation2d(0.01, 0.01)); - private static VisionSystem[] systemList = - new VisionSystem[Constants.Vision.Cameras.values().length]; - private Transform3d[] camToRobots = { - // right Camera transform - new Transform3d( - new Translation3d( - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_X, - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_Y, - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_Z), - new Rotation3d( - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_ROLL, - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_PITCH, - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_YAW)), - // left Camera - new Transform3d( - new Translation3d( - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_X, - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_Y, - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_Z), - new Rotation3d( - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_ROLL, - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_PITCH, - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_YAW)), - }; - private PhotonCamera camera; - private Constants.Vision.Cameras cameraEnum; - private PhotonPipelineResult pipeline; - AprilTagFieldLayout aprilTagFieldLayout = + // Reef tag IDs for each side of the field + private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); + private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); + + // Base noise tuning parameters (tweakable) + private double calibrationFactor = 1.0; + private double baseNoiseX = 0.1; // meters + private double baseNoiseY = 0.1; + private double baseNoiseTheta = 0.17; // radians + + private double distanceCoefficientX = 0.02; // noise growth per meter + private double distanceCoefficientY = 0.02; + private double distanceCoefficientTheta = 0.01; + + private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle + private double angleCoefficientY = 0.5; + private double angleCoefficientTheta = 0.5; + + private double speedCoefficientX = 0.5; // noise growth per fraction of max speed + private double speedCoefficientY = 0.5; + private double speedCoefficientTheta = 0.2; + + // Maximums for normalization + private double maximumViewingAngle = Math.toRadians(90.0); + private double maximumRobotSpeed = 3.0; // meters per second + private double maximumAllowedDistance = 5.0; // meters, beyond which readings are dropped + + // PhotonVision and odometry references + private final PhotonCamera photonCamera; + private final PhotonPoseEstimator poseEstimator; + private PhotonPipelineResult latestVisionResult; + private final BooleanSupplier isRedSide; + private Pose2d lastKnownPose = new Pose2d(0, 0, new Rotation2d()); + private final SwerveSubsystem swerveDrive = SwerveSubsystem.getInstance(); + private final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - PhotonPoseEstimator photonPoseEstimator; - private SwerveSubsystem driveTrain = SwerveSubsystem.getInstance(); - private BooleanSupplier redSide; - public VisionSystem(Constants.Vision.Cameras cameraEnum, BooleanSupplier redSide) { - this.cameraEnum = cameraEnum; - String name = cameraEnum.toString(); - int index = cameraEnum.ordinal(); - camera = new PhotonCamera(name); - photonPoseEstimator = + public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + this.isRedSide = isRedSide; + photonCamera = new PhotonCamera(cameraId.toString()); + Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); + poseEstimator = new PhotonPoseEstimator( - aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camToRobots[index]); - this.redSide = redSide; - for (var x : camera.getAllUnreadResults()) { - pipeline = x; - } + fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + latestVisionResult = null; } - public Double getDistance() { - return this.getAprilTagFieldLayout() - .getTagPose(getPipelineResult().getBestTarget().getFiducialId()) - .get() - .getTranslation() - .getDistance( - new Translation3d( - driveTrain.getState().Pose.getX(), driveTrain.getState().Pose.getY(), 0.0)); + @Override + public void periodic() { + for (var x : photonCamera.getAllUnreadResults()) { + latestVisionResult = x; + } } - public static VisionSystem getInstance(Constants.Vision.Cameras name, BooleanSupplier redSide) { - if (systemList[name.ordinal()] == null) { - systemList[name.ordinal()] = new VisionSystem(name, redSide); + /** + * Attempts to fuse a vision measurement into the swerve pose estimator, dropping readings that + * fail validity checks, and computing noise dynamically via computeMeasurementNoise(). + */ + public void addFilteredPose() { + if (latestVisionResult == null || !latestVisionResult.hasTargets()) { + DogLog.log("Vision/MeasurementUsed", false); + return; } - return systemList[name.ordinal()]; - } + // Filter tags to only those on the active side + List validTags = + latestVisionResult.getTargets().stream() + .filter(t -> isTagOnActiveSide(t.getFiducialId())) + .collect(Collectors.toList()); + if (validTags.isEmpty()) { + DogLog.log("Vision/TagFilter", false); + return; + } - // use this when feeding into drivetrain - public Optional getMultiTagPose3d(Pose2d previousRobotPose) { - photonPoseEstimator.setReferencePose(previousRobotPose); - return photonPoseEstimator.update(pipeline); + // Compute effective metrics for multi-tag solution + int tagCount = validTags.size(); + double averageDistance = + validTags.stream() + .mapToDouble(t -> getDistanceToTag(t.getFiducialId())) + .average() + .orElse(Double.NaN); + if (Double.isNaN(averageDistance) || averageDistance > maximumAllowedDistance) { + DogLog.log("Vision/DistanceFilter", true); + return; + } + double averageAngle = + validTags.stream() + .mapToDouble(PhotonTrackedTarget::getYaw) + .map(Math::toRadians) + .map(Math::abs) + .average() + .orElse(0.0); + double currentSpeed = + Math.hypot( + swerveDrive.getRobotSpeeds().vxMetersPerSecond, + swerveDrive.getRobotSpeeds().vyMetersPerSecond); + + // Compute measurement noise for each axis + double noiseX = + computeMeasurementNoise( + baseNoiseX, + distanceCoefficientX, + angleCoefficientX, + speedCoefficientX, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + double noiseY = + computeMeasurementNoise( + baseNoiseY, + distanceCoefficientY, + angleCoefficientY, + speedCoefficientY, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + double noiseTheta = + computeMeasurementNoise( + baseNoiseTheta, + distanceCoefficientTheta, + angleCoefficientTheta, + speedCoefficientTheta, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + + DogLog.log("Vision/NoiseX", noiseX); + DogLog.log("Vision/NoiseY", noiseY); + DogLog.log("Vision/NoiseTheta", noiseTheta); + + // Get the pose from PhotonVision + poseEstimator.setReferencePose(lastKnownPose); + Optional maybePose = poseEstimator.update(latestVisionResult); + if (maybePose.isEmpty()) { + DogLog.log("Vision/PoseEstimateFailed", true); + return; + } + Pose2d measuredPose = maybePose.get().estimatedPose.toPose2d(); + lastKnownPose = measuredPose; + + // Choose timestamp: use vision timestamp unless it differs too much from FPGA + double visionTimestamp = latestVisionResult.getTimestampSeconds(); + double fpgaTimestamp = Timer.getFPGATimestamp(); + double timestampDifference = Math.abs(visionTimestamp - fpgaTimestamp); + double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp : visionTimestamp; + + // Build the noise vector and add the vision measurement + Matrix noiseVector = VecBuilder.fill(noiseX, noiseY, noiseTheta); + swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); + DogLog.log("Vision/MeasurementUsed", true); } - public Pose2d getPose2d() { - Optional pose3d = getMultiTagPose3d(savedResult); - if (pose3d.isEmpty()) return null; - savedResult = pose3d.get().estimatedPose.toPose2d(); - return savedResult; + private boolean isTagOnActiveSide(int tagId) { + return isRedSide.getAsBoolean() + ? RED_SIDE_TAG_IDS.contains(tagId) + : BLUE_SIDE_TAG_IDS.contains(tagId); } - public Pose2d getSaved() { - return savedResult; + private double getDistanceToTag(int tagId) { + return fieldLayout + .getTagPose(tagId) + .map( + pose3d -> + pose3d + .getTranslation() + .getDistance( + new Translation3d( + swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), 0.0))) + .orElse(Double.NaN); } - public boolean hasTarget(PhotonPipelineResult pipeline) { - if (pipeline == null) { - return false; - } - return pipeline.hasTargets(); + /** + * Computes a measurement noise standard deviation based on: - base noise (zero-distance, head-on) + * - distance scaling - viewing angle scaling - robot speed scaling - tag count (noise reduction + * by sqrt(N)) + */ + private double computeMeasurementNoise( + double baseNoise, + double distanceCoefficient, + double angleCoefficient, + double speedCoefficient, + double distance, + double angleRad, + double robotSpeed, + int tagCount) { + double tagCountScale = 1.0 / Math.sqrt(Math.max(tagCount, 1)); + double distanceTerm = baseNoise + distanceCoefficient * distance; + double angleTerm = 1.0 + angleCoefficient * (angleRad / maximumViewingAngle); + double speedTerm = 1.0 + speedCoefficient * (robotSpeed / maximumRobotSpeed); + return calibrationFactor * tagCountScale * distanceTerm * angleTerm * speedTerm; } - public void setReference(Pose2d newPose) { - if (newPose == null) { - return; - } - savedResult = newPose; + // Setters for runtime tuning of parameters + public void setCalibrationFactor(double factor) { + calibrationFactor = factor; } - public static Pose2d getAverageForOffBotTesting(Pose2d one, Pose2d two) { - if (one == null) return two; - if (two == null) return one; - - return new Pose2d( - (one.getX() + two.getX()) / 2, (one.getY() + two.getY()) / 2, one.getRotation()); + public void setBaseNoise(double noiseX, double noiseY, double noiseTheta) { + baseNoiseX = noiseX; + baseNoiseY = noiseY; + baseNoiseTheta = noiseTheta; } - public AprilTagFieldLayout getAprilTagFieldLayout() { - return this.aprilTagFieldLayout; + public void setDistanceCoefficients(double coeffX, double coeffY, double coeffTheta) { + distanceCoefficientX = coeffX; + distanceCoefficientY = coeffY; + distanceCoefficientTheta = coeffTheta; } - public PhotonPipelineResult getPipelineResult() { - return pipeline; + public void setAngleCoefficients(double coeffX, double coeffY, double coeffTheta) { + angleCoefficientX = coeffX; + angleCoefficientY = coeffY; + angleCoefficientTheta = coeffTheta; } - public void addFilteredPose() { - String camName = cameraEnum.getLoggingName(); - PhotonPipelineResult pipelineResult = getPipelineResult(); - - DogLog.log("KalmanDebug/" + camName + "PiplineNull", pipelineResult == null); - DogLog.log("KalmanDebug/" + camName + "PipelineHasTarget", hasTarget(pipelineResult)); - - if (pipelineResult != null && hasTarget(pipelineResult)) { - - List targets = pipelineResult.getTargets(); - - boolean hasReefTag = true; - double poseAmbiguity = pipelineResult.getBestTarget().poseAmbiguity; - double distance = getDistance(); - - DogLog.log("KalmanDebug/" + camName + "PoseAmbiguity", poseAmbiguity); - DogLog.log("KalmanDebug/" + camName + "DistToAprilTag", distance); - - for (PhotonTrackedTarget target : targets) { - DogLog.log("redside in vision system", redSide.getAsBoolean()); - if (redSide.getAsBoolean()) { - if (!redReefID.contains(target.getFiducialId())) { - hasReefTag = false; - } - } else { - if (!blueReefID.contains(target.getFiducialId())) { - hasReefTag = false; - } - } - // if (!reefIDs.contains(target.getFiducialId())) { - // hasReefTag = false; - // } - } - - if (hasReefTag) { - double speedMultiplier = - (Math.sqrt( - Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2))) - / 2 - + 1.0; - double xKalman = MiscUtils.lerp((distance - 0.62) / 3, 0.03, 0.3, 1.0) * speedMultiplier; - double yKalman = MiscUtils.lerp((distance - 0.62) / 3, 0.03, 0.3, 1.0) * speedMultiplier; - // double speedMultiplier = 1; - // if (Math.sqrt( - // Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - // + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2)) - // > 1) { - // speedMultiplier = 2; - // } - // speedMultiplier = - // (Math.sqrt( - // Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - // + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2))) - // / 2 - // + 1.0; - // double xKalman = MiscUtils.lerp((distance - 0.6) / 2.4, 0.04, 0.5, 1.0) * - // speedMultiplier; - // double yKalman = MiscUtils.lerp((distance - 0.6) / 2.4, 0.04, 0.5, 1.0) * - // speedMultiplier; - double rotationKalman = MiscUtils.lerp((distance - 0.6) / 1.4, 0.4, 5, 30) / 10; - - DogLog.log("KalmanDebug/" + camName + "TranslationStandardDeviation", xKalman); - DogLog.log("KalmanDebug/" + camName + "RotationStandardDeviation", rotationKalman); - - Matrix visionMatrix = VecBuilder.fill(xKalman, yKalman, rotationKalman); - Pose2d bestRobotPose2d = getPose2d(); - - if (bestRobotPose2d == null) { - VisionSystem.numThrowaways++; - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - DogLog.log("KalmanDebug/" + camName + "throwaways", VisionSystem.numThrowaways); - return; - } - Pose2d rotationLess = - new Pose2d(bestRobotPose2d.getTranslation(), driveTrain.getState().Pose.getRotation()); - DogLog.log("KalmanDebug/" + camName + "Rotationless", rotationLess); - DogLog.log("KalmanDebug/" + camName + "RobotPoseIsPresent", bestRobotPose2d != null); - DogLog.log("KalmanDebug/" + camName + "VisionPose", bestRobotPose2d); - - double xDifference = Math.abs(driveTrain.getPose().getX() - bestRobotPose2d.getX()); - double yDifference = Math.abs(driveTrain.getPose().getY() - bestRobotPose2d.getY()); - double rotDifference = - Math.abs( - driveTrain.getPose().getRotation().getDegrees() - - bestRobotPose2d.getRotation().getDegrees()); - Translation2d transDifference = new Translation2d(xDifference, yDifference); - Rotation2d rot2dDifference = new Rotation2d(rotDifference); - - Pose2d visionDiff = new Pose2d(transDifference, rot2dDifference); - double timeDiff = Math.abs(pipelineResult.getTimestampSeconds() - Timer.getTimestamp()); - DogLog.log("KalmanDebug/" + camName + "visionDiff", visionDiff); - DogLog.log("KalmanDebug/" + camName + "diffInTimestamps", timeDiff); - - driveTrain.addVisionMeasurement( - bestRobotPose2d, - timeDiff > 5 ? Timer.getTimestamp() - 0.070 : pipelineResult.getTimestampSeconds(), - visionMatrix); - VisionSystem.numNotThrowaways++; - DogLog.log("KalmanDebug/" + camName + "numNotThrowaway", VisionSystem.numNotThrowaways); - DogLog.log("KalmanDebug/" + camName + "visionUsed", true); - } else { - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - } - } else { - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - } + public void setSpeedCoefficients(double coeffX, double coeffY, double coeffTheta) { + speedCoefficientX = coeffX; + speedCoefficientY = coeffY; + speedCoefficientTheta = coeffTheta; } - @Override - public void periodic() { - for (var x : camera.getAllUnreadResults()) { - pipeline = x; - } + public void setMaximums(double maxDistance, double maxSpeed, double maxAngleDegrees) { + maximumAllowedDistance = maxDistance; + maximumRobotSpeed = maxSpeed; + maximumViewingAngle = Math.toRadians(maxAngleDegrees); } } From ebd19e97ef889261d0332bd578add8846a85dd76 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 12 Jul 2025 16:52:00 -0700 Subject: [PATCH 02/43] updated max speed --- src/main/java/frc/robot/subsystems/VisionSystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 3d786484..772df834 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -59,7 +59,7 @@ public class VisionSystem extends SubsystemBase { // Maximums for normalization private double maximumViewingAngle = Math.toRadians(90.0); - private double maximumRobotSpeed = 3.0; // meters per second + private double maximumRobotSpeed = 4.8; // meters per second private double maximumAllowedDistance = 5.0; // meters, beyond which readings are dropped // PhotonVision and odometry references From a77d2fd6de454929e8249b10a7bc42ee387d8ffd Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 19 Jul 2025 16:39:46 -0700 Subject: [PATCH 03/43] restored singleton & system camera list --- src/main/java/frc/robot/subsystems/VisionSystem.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 772df834..6031d060 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -35,6 +35,8 @@ * speed. */ public class VisionSystem extends SubsystemBase { + private static VisionSystem[] systemList = new VisionSystem[Constants.Vision.Cameras.values().length]; + // Reef tag IDs for each side of the field private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); @@ -82,6 +84,14 @@ public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide latestVisionResult = null; } + public static VisionSystem getInstance(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + int idx = cameraId.ordinal(); + if (systemList[idx] == null) { + systemList[idx] = new VisionSystem(cameraId, isRedSide); + } + return systemList[idx]; + } + @Override public void periodic() { for (var x : photonCamera.getAllUnreadResults()) { From 7a73160404015e41771989dd688b5f747f6e2b15 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 19 Jul 2025 18:34:27 -0700 Subject: [PATCH 04/43] With nt publish enabled (thank you setty) --- src/main/java/frc/robot/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1743b9ec..4ec85608 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -89,7 +89,7 @@ public void disabledInit() { @Override public void robotInit() { DogLog.setOptions( - new DogLogOptions().withNtPublish(false).withCaptureDs(true).withLogExtras(true)); + new DogLogOptions().withNtPublish(true).withCaptureDs(true).withLogExtras(true)); DogLog.log("PIDArmKP", Constants.Arm.S0C_KP); DogLog.log("PIDArmKI", Constants.Arm.S0C_KI); DogLog.log("PIDArmKD", Constants.Arm.S0C_KD); From 7705dcc1f7ae3d714139de9e0a097cb5cad181c5 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 19 Jul 2025 18:34:47 -0700 Subject: [PATCH 05/43] disabeled this to get rid of an error --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61045abc..7823a556 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -107,8 +107,8 @@ public void teleopInit() { } private void configureBindings() { - leds.setDefaultCommand( - new InstantCommand(() -> leds.updateLedsCommand(LedSubsystem.LedState.IDLE))); + // leds.setDefaultCommand( + // new InstantCommand(() -> leds.updateLedsCommand(LedSubsystem.LedState.IDLE))); armSubsystem.setDefaultCommand(new ArmToAngleCmd(0.0, armSubsystem)); elevatorSubsystem.setDefaultCommand(new DefaultElevator(elevatorSubsystem)); From 4afb05be626ebb22f2ff34f8257310133030b800 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 19 Jul 2025 18:35:15 -0700 Subject: [PATCH 06/43] vision system singleton camera systemlist --- .../java/frc/robot/subsystems/VisionSystem.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 6031d060..1acd31d0 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -35,7 +35,8 @@ * speed. */ public class VisionSystem extends SubsystemBase { - private static VisionSystem[] systemList = new VisionSystem[Constants.Vision.Cameras.values().length]; + private static VisionSystem[] systemList = + new VisionSystem[Constants.Vision.Cameras.values().length]; // Reef tag IDs for each side of the field private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); @@ -43,13 +44,13 @@ public class VisionSystem extends SubsystemBase { // Base noise tuning parameters (tweakable) private double calibrationFactor = 1.0; - private double baseNoiseX = 0.1; // meters - private double baseNoiseY = 0.1; - private double baseNoiseTheta = 0.17; // radians + private double baseNoiseX = 0.05; // meters + private double baseNoiseY = 0.05; + private double baseNoiseTheta = 1; // radians private double distanceCoefficientX = 0.02; // noise growth per meter private double distanceCoefficientY = 0.02; - private double distanceCoefficientTheta = 0.01; + private double distanceCoefficientTheta = 1; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle private double angleCoefficientY = 0.5; @@ -62,7 +63,7 @@ public class VisionSystem extends SubsystemBase { // Maximums for normalization private double maximumViewingAngle = Math.toRadians(90.0); private double maximumRobotSpeed = 4.8; // meters per second - private double maximumAllowedDistance = 5.0; // meters, beyond which readings are dropped + private double maximumAllowedDistance = 15.0; // meters, beyond which readings are dropped // PhotonVision and odometry references private final PhotonCamera photonCamera; @@ -84,7 +85,8 @@ public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide latestVisionResult = null; } - public static VisionSystem getInstance(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + public static VisionSystem getInstance( + Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { int idx = cameraId.ordinal(); if (systemList[idx] == null) { systemList[idx] = new VisionSystem(cameraId, isRedSide); From 2bf0c79b25c2539f9dcb3b1a1572d55b58ebf372 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 19 Jul 2025 19:53:53 -0700 Subject: [PATCH 07/43] camera-specific vision logging --- .../frc/robot/subsystems/VisionSystem.java | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 1acd31d0..6719fcd9 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -38,18 +38,19 @@ public class VisionSystem extends SubsystemBase { private static VisionSystem[] systemList = new VisionSystem[Constants.Vision.Cameras.values().length]; + private final Constants.Vision.Cameras cameraId; // Reef tag IDs for each side of the field private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); // Base noise tuning parameters (tweakable) private double calibrationFactor = 1.0; - private double baseNoiseX = 0.05; // meters - private double baseNoiseY = 0.05; - private double baseNoiseTheta = 1; // radians + private double baseNoiseX = 0.02; // meters + private double baseNoiseY = 0.02; + private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.02; // noise growth per meter - private double distanceCoefficientY = 0.02; + private double distanceCoefficientX = 0.4; // noise growth per meter + private double distanceCoefficientY = 0.4; private double distanceCoefficientTheta = 1; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle @@ -58,11 +59,11 @@ public class VisionSystem extends SubsystemBase { private double speedCoefficientX = 0.5; // noise growth per fraction of max speed private double speedCoefficientY = 0.5; - private double speedCoefficientTheta = 0.2; + private double speedCoefficientTheta = 0.5; // Maximums for normalization private double maximumViewingAngle = Math.toRadians(90.0); - private double maximumRobotSpeed = 4.8; // meters per second + private double maximumRobotSpeed = 5; // meters per second private double maximumAllowedDistance = 15.0; // meters, beyond which readings are dropped // PhotonVision and odometry references @@ -77,6 +78,7 @@ public class VisionSystem extends SubsystemBase { public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { this.isRedSide = isRedSide; + this.cameraId = cameraId; photonCamera = new PhotonCamera(cameraId.toString()); Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); poseEstimator = @@ -189,7 +191,7 @@ public void addFilteredPose() { } Pose2d measuredPose = maybePose.get().estimatedPose.toPose2d(); lastKnownPose = measuredPose; - + DogLog.log("Vision/" + cameraId.toString() + "Pose", measuredPose); // Choose timestamp: use vision timestamp unless it differs too much from FPGA double visionTimestamp = latestVisionResult.getTimestampSeconds(); double fpgaTimestamp = Timer.getFPGATimestamp(); From 3cac2f125132f07e89683c9e204a0ee4ee14de9f Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 2 Aug 2025 12:47:03 -0700 Subject: [PATCH 08/43] first commit --- .../frc/robot/subsystems/VisionSystem.java | 228 ++++++++++++++++-- 1 file changed, 204 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 6719fcd9..6d30951c 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -68,10 +68,12 @@ public class VisionSystem extends SubsystemBase { // PhotonVision and odometry references private final PhotonCamera photonCamera; - private final PhotonPoseEstimator poseEstimator; + private final PhotonPoseEstimator poseEstimator; // MULTI_TAG_PNP_ON_COPROCESSOR + private final PhotonPoseEstimator trigsolvePoseEstimator; // LOWEST_AMBIGUITY with trigsolve private PhotonPipelineResult latestVisionResult; private final BooleanSupplier isRedSide; private Pose2d lastKnownPose = new Pose2d(0, 0, new Rotation2d()); + private Pose2d lastKnownTrigsolvePose = new Pose2d(0, 0, new Rotation2d()); private final SwerveSubsystem swerveDrive = SwerveSubsystem.getInstance(); private final AprilTagFieldLayout fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); @@ -81,9 +83,16 @@ public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide this.cameraId = cameraId; photonCamera = new PhotonCamera(cameraId.toString()); Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); + + // Initialize both pose estimators poseEstimator = new PhotonPoseEstimator( fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + + trigsolvePoseEstimator = + new PhotonPoseEstimator( + fieldLayout, PoseStrategy.LOWEST_AMBIGUITY, cameraToRobot); + latestVisionResult = null; } @@ -103,14 +112,37 @@ public void periodic() { } } + /** + * Attempts to fuse a vision measurement into the swerve pose estimator using MULTI_TAG_PNP strategy, + * dropping readings that fail validity checks, and computing noise dynamically via computeMeasurementNoise(). + */ + public void addFilteredPose() { + addFilteredPose(false); + } + /** * Attempts to fuse a vision measurement into the swerve pose estimator, dropping readings that * fail validity checks, and computing noise dynamically via computeMeasurementNoise(). + * + * @param useTrigsolve If true, uses the trigsolve pose estimator (PNP_DISTANCE_TRIG_SOLVE strategy). + * If false, uses the multi-tag PnP pose estimator (MULTI_TAG_PNP_ON_COPROCESSOR strategy). */ - public void addFilteredPose() { + public void addFilteredPose(boolean useTrigsolve) { + addFilteredPoseInternal(useTrigsolve, false); + } + + /** + * Internal method to handle pose estimation with optional confidence checking. + * + * @param useTrigsolve If true, uses trigsolve strategy + * @param forceAdd If true, bypasses confidence checking (used for non-trigsolve or single camera) + * @return PoseEstimateResult containing the pose and confidence data, or null if failed + */ + private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean forceAdd) { if (latestVisionResult == null || !latestVisionResult.hasTargets()) { + if (forceAdd) return null; DogLog.log("Vision/MeasurementUsed", false); - return; + return null; } // Filter tags to only those on the active side @@ -119,11 +151,12 @@ public void addFilteredPose() { .filter(t -> isTagOnActiveSide(t.getFiducialId())) .collect(Collectors.toList()); if (validTags.isEmpty()) { + if (forceAdd) return null; DogLog.log("Vision/TagFilter", false); - return; + return null; } - // Compute effective metrics for multi-tag solution + // Compute effective metrics for solution int tagCount = validTags.size(); double averageDistance = validTags.stream() @@ -131,8 +164,9 @@ public void addFilteredPose() { .average() .orElse(Double.NaN); if (Double.isNaN(averageDistance) || averageDistance > maximumAllowedDistance) { + if (forceAdd) return null; DogLog.log("Vision/DistanceFilter", true); - return; + return null; } double averageAngle = validTags.stream() @@ -146,6 +180,126 @@ public void addFilteredPose() { swerveDrive.getRobotSpeeds().vxMetersPerSecond, swerveDrive.getRobotSpeeds().vyMetersPerSecond); + // Choose the appropriate pose estimator and reference pose + PhotonPoseEstimator selectedEstimator = useTrigsolve ? trigsolvePoseEstimator : poseEstimator; + Pose2d referencePos = useTrigsolve ? lastKnownTrigsolvePose : lastKnownPose; + + // Get the pose from PhotonVision + selectedEstimator.setReferencePose(referencePos); + Optional maybePose = selectedEstimator.update(latestVisionResult); + if (maybePose.isEmpty()) { + if (forceAdd) return null; + String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; + DogLog.log("Vision/" + strategyPrefix + "PoseEstimateFailed", true); + return null; + } + + EstimatedRobotPose estimatedPose = maybePose.get(); + Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); + + // Calculate confidence metric for trigsolve based on measurement quality + double confidence = 1.0; // Default confidence for multi-tag + if (useTrigsolve) { + // For trigsolve, calculate confidence based on the measurement noise multipliers + // Lower total noise = higher confidence + double noiseX = + computeMeasurementNoise( + baseNoiseX, + distanceCoefficientX, + angleCoefficientX, + speedCoefficientX, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + double noiseY = + computeMeasurementNoise( + baseNoiseY, + distanceCoefficientY, + angleCoefficientY, + speedCoefficientY, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + double noiseTheta = + computeMeasurementNoise( + baseNoiseTheta, + distanceCoefficientTheta, + angleCoefficientTheta, + speedCoefficientTheta, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + + // Calculate overall measurement quality (lower noise = higher confidence) + double totalNoise = Math.sqrt(noiseX * noiseX + noiseY * noiseY + noiseTheta * noiseTheta); + confidence = 1.0 / (1.0 + totalNoise); + } + + // If this is just for confidence comparison, return the result without adding to odometry + if (forceAdd) { + return new PoseEstimateResult(measuredPose, confidence, averageDistance, averageAngle, + currentSpeed, tagCount, latestVisionResult.getTimestampSeconds()); + } + + // For non-trigsolve or when not doing confidence comparison, proceed normally + if (!useTrigsolve) { + return processPoseEstimate(measuredPose, useTrigsolve, averageDistance, averageAngle, + currentSpeed, tagCount, latestVisionResult.getTimestampSeconds()); + } + + // For trigsolve, check confidence against other cameras + return processTrigsolveWithConfidenceCheck(measuredPose, confidence, averageDistance, + averageAngle, currentSpeed, tagCount, + latestVisionResult.getTimestampSeconds()); + } + + /** + * Processes trigsolve pose estimation with confidence checking across all cameras. + */ + private PoseEstimateResult processTrigsolveWithConfidenceCheck(Pose2d measuredPose, double confidence, + double averageDistance, double averageAngle, double currentSpeed, int tagCount, double timestamp) { + + // Get results from all other cameras for confidence comparison + double bestConfidence = confidence; + PoseEstimateResult bestResult = new PoseEstimateResult(measuredPose, confidence, averageDistance, + averageAngle, currentSpeed, tagCount, timestamp); + + // Check all other camera instances + for (Constants.Vision.Cameras otherCamera : Constants.Vision.Cameras.values()) { + if (otherCamera == this.cameraId) continue; // Skip self + + VisionSystem otherSystem = systemList[otherCamera.ordinal()]; + if (otherSystem != null) { + PoseEstimateResult otherResult = otherSystem.addFilteredPoseInternal(true, true); + if (otherResult != null && otherResult.confidence > bestConfidence) { + bestConfidence = otherResult.confidence; + bestResult = otherResult; + } + } + } + + // Only proceed if this camera has the best confidence + if (bestResult.pose == measuredPose) { // This camera won + return processPoseEstimate(measuredPose, true, averageDistance, averageAngle, + currentSpeed, tagCount, timestamp); + } else { + // Another camera had better confidence, don't add measurement + DogLog.log("Vision/TrigsolveLowerConfidence", true); + DogLog.log("Vision/TrigsolveOurConfidence", confidence); + DogLog.log("Vision/TrigsolveBestConfidence", bestConfidence); + return null; + } + } + + /** + * Final processing and addition of pose estimate to odometry. + */ + private PoseEstimateResult processPoseEstimate(Pose2d measuredPose, boolean useTrigsolve, + double averageDistance, double averageAngle, double currentSpeed, int tagCount, double timestamp) { + // Compute measurement noise for each axis double noiseX = computeMeasurementNoise( @@ -178,30 +332,32 @@ public void addFilteredPose() { currentSpeed, tagCount); - DogLog.log("Vision/NoiseX", noiseX); - DogLog.log("Vision/NoiseY", noiseY); - DogLog.log("Vision/NoiseTheta", noiseTheta); - - // Get the pose from PhotonVision - poseEstimator.setReferencePose(lastKnownPose); - Optional maybePose = poseEstimator.update(latestVisionResult); - if (maybePose.isEmpty()) { - DogLog.log("Vision/PoseEstimateFailed", true); - return; + String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; + DogLog.log("Vision/" + strategyPrefix + "NoiseX", noiseX); + DogLog.log("Vision/" + strategyPrefix + "NoiseY", noiseY); + DogLog.log("Vision/" + strategyPrefix + "NoiseTheta", noiseTheta); + + // Update the appropriate reference pose + if (useTrigsolve) { + lastKnownTrigsolvePose = measuredPose; + } else { + lastKnownPose = measuredPose; } - Pose2d measuredPose = maybePose.get().estimatedPose.toPose2d(); - lastKnownPose = measuredPose; - DogLog.log("Vision/" + cameraId.toString() + "Pose", measuredPose); + + DogLog.log("Vision/" + cameraId.toString() + strategyPrefix + "Pose", measuredPose); + // Choose timestamp: use vision timestamp unless it differs too much from FPGA - double visionTimestamp = latestVisionResult.getTimestampSeconds(); double fpgaTimestamp = Timer.getFPGATimestamp(); - double timestampDifference = Math.abs(visionTimestamp - fpgaTimestamp); - double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp : visionTimestamp; + double timestampDifference = Math.abs(timestamp - fpgaTimestamp); + double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp : timestamp; // Build the noise vector and add the vision measurement Matrix noiseVector = VecBuilder.fill(noiseX, noiseY, noiseTheta); swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); - DogLog.log("Vision/MeasurementUsed", true); + DogLog.log("Vision/" + strategyPrefix + "MeasurementUsed", true); + + return new PoseEstimateResult(measuredPose, 1.0, averageDistance, averageAngle, + currentSpeed, tagCount, timestamp); } private boolean isTagOnActiveSide(int tagId) { @@ -278,4 +434,28 @@ public void setMaximums(double maxDistance, double maxSpeed, double maxAngleDegr maximumRobotSpeed = maxSpeed; maximumViewingAngle = Math.toRadians(maxAngleDegrees); } -} + + /** + * Helper class to store pose estimation results with confidence metrics. + */ + private static class PoseEstimateResult { + public final Pose2d pose; + public final double confidence; + public final double averageDistance; + public final double averageAngle; + public final double currentSpeed; + public final int tagCount; + public final double timestamp; + + public PoseEstimateResult(Pose2d pose, double confidence, double averageDistance, + double averageAngle, double currentSpeed, int tagCount, double timestamp) { + this.pose = pose; + this.confidence = confidence; + this.averageDistance = averageDistance; + this.averageAngle = averageAngle; + this.currentSpeed = currentSpeed; + this.tagCount = tagCount; + this.timestamp = timestamp; + } + } +} \ No newline at end of file From 9bbb5c8d2f36f33e975658ab5c1fb16c271a9ed1 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 2 Aug 2025 13:44:53 -0700 Subject: [PATCH 09/43] total trigsolve use test --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4ec85608..221eb58e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -71,8 +71,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); m_robotContainer.doTelemetry(); - visionRight.addFilteredPose(); - visionLeft.addFilteredPose(); + visionRight.addFilteredPose(true); + visionLeft.addFilteredPose(true); DogLog.log("KalmanDebug/drivetrainPose", driveTrain.getPose()); From d1d92bbc32e4a28ff228efed40d394f75faca654 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 2 Aug 2025 13:58:25 -0700 Subject: [PATCH 10/43] took away trig solve --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 221eb58e..4ec85608 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -71,8 +71,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); m_robotContainer.doTelemetry(); - visionRight.addFilteredPose(true); - visionLeft.addFilteredPose(true); + visionRight.addFilteredPose(); + visionLeft.addFilteredPose(); DogLog.log("KalmanDebug/drivetrainPose", driveTrain.getPose()); From c6b1188da7a469b5bc33c01d10315edfde058938 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 2 Aug 2025 14:18:49 -0700 Subject: [PATCH 11/43] add back in trig --- src/main/java/frc/robot/Robot.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 4ec85608..221eb58e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -71,8 +71,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); m_robotContainer.doTelemetry(); - visionRight.addFilteredPose(); - visionLeft.addFilteredPose(); + visionRight.addFilteredPose(true); + visionLeft.addFilteredPose(true); DogLog.log("KalmanDebug/drivetrainPose", driveTrain.getPose()); From 0b4e3216ddf64f5fb74b730164fa947fd9863b5f Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 14:32:25 -0700 Subject: [PATCH 12/43] trigsolve --- .../frc/robot/subsystems/VisionSystem.java | 38 ++++++++++++++----- 1 file changed, 29 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 6d30951c..e21f79e5 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -45,12 +45,12 @@ public class VisionSystem extends SubsystemBase { // Base noise tuning parameters (tweakable) private double calibrationFactor = 1.0; - private double baseNoiseX = 0.02; // meters - private double baseNoiseY = 0.02; + private double baseNoiseX = 0.01; // meters + private double baseNoiseY = 0.01; private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.4; // noise growth per meter - private double distanceCoefficientY = 0.4; + private double distanceCoefficientX = 0.0444898; // noise growth per meter + private double distanceCoefficientY = 0.0444898; private double distanceCoefficientTheta = 1; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle @@ -91,7 +91,7 @@ public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide trigsolvePoseEstimator = new PhotonPoseEstimator( - fieldLayout, PoseStrategy.LOWEST_AMBIGUITY, cameraToRobot); + fieldLayout, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, cameraToRobot); latestVisionResult = null; } @@ -168,13 +168,28 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean DogLog.log("Vision/DistanceFilter", true); return null; } - double averageAngle = + double averageYawDeg = validTags.stream() .mapToDouble(PhotonTrackedTarget::getYaw) - .map(Math::toRadians) .map(Math::abs) .average() .orElse(0.0); + double averagePitchDeg = + validTags.stream() + .mapToDouble(PhotonTrackedTarget::getPitch) + .map(Math::abs) + .average() + .orElse(0.0); + + // Reject overly oblique views (>70 degrees in either axis) + if (averageYawDeg > 70.0 || averagePitchDeg > 70.0) { + if (forceAdd) return null; + DogLog.log("Vision/ViewAngleTooLarge", true); + return null; + } + + // retain previous semantics for noise calculation (uses yaw) + double averageAngle = Math.toRadians(averageYawDeg); double currentSpeed = Math.hypot( swerveDrive.getRobotSpeeds().vxMetersPerSecond, @@ -394,8 +409,13 @@ private double computeMeasurementNoise( double robotSpeed, int tagCount) { double tagCountScale = 1.0 / Math.sqrt(Math.max(tagCount, 1)); - double distanceTerm = baseNoise + distanceCoefficient * distance; - double angleTerm = 1.0 + angleCoefficient * (angleRad / maximumViewingAngle); + double distanceTerm = baseNoise + distanceCoefficient * distance * distance; + double cosMax = Math.cos(maximumViewingAngle); + double normalizedAngle = 0.0; + if (1.0 - cosMax > 1e-9) { + normalizedAngle = (1.0 - Math.cos(angleRad)) / (1.0 - cosMax); + } + double angleTerm = 1.0 + angleCoefficient * normalizedAngle; double speedTerm = 1.0 + speedCoefficient * (robotSpeed / maximumRobotSpeed); return calibrationFactor * tagCountScale * distanceTerm * angleTerm * speedTerm; } From 4f795fff655b567616db6f0ef61fdfeeb58c97e2 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 14:39:22 -0700 Subject: [PATCH 13/43] removed a comment --- src/main/java/frc/robot/subsystems/VisionSystem.java | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index e21f79e5..8754c881 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -394,11 +394,6 @@ private double getDistanceToTag(int tagId) { .orElse(Double.NaN); } - /** - * Computes a measurement noise standard deviation based on: - base noise (zero-distance, head-on) - * - distance scaling - viewing angle scaling - robot speed scaling - tag count (noise reduction - * by sqrt(N)) - */ private double computeMeasurementNoise( double baseNoise, double distanceCoefficient, From de6c4cefe7703a0b46ca77e8c96152c8bedd275b Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 14:41:57 -0700 Subject: [PATCH 14/43] removed setters that shouldn't be there --- .../frc/robot/subsystems/VisionSystem.java | 36 ------------------- 1 file changed, 36 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 8754c881..9de9f0a7 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -414,42 +414,6 @@ private double computeMeasurementNoise( double speedTerm = 1.0 + speedCoefficient * (robotSpeed / maximumRobotSpeed); return calibrationFactor * tagCountScale * distanceTerm * angleTerm * speedTerm; } - - // Setters for runtime tuning of parameters - public void setCalibrationFactor(double factor) { - calibrationFactor = factor; - } - - public void setBaseNoise(double noiseX, double noiseY, double noiseTheta) { - baseNoiseX = noiseX; - baseNoiseY = noiseY; - baseNoiseTheta = noiseTheta; - } - - public void setDistanceCoefficients(double coeffX, double coeffY, double coeffTheta) { - distanceCoefficientX = coeffX; - distanceCoefficientY = coeffY; - distanceCoefficientTheta = coeffTheta; - } - - public void setAngleCoefficients(double coeffX, double coeffY, double coeffTheta) { - angleCoefficientX = coeffX; - angleCoefficientY = coeffY; - angleCoefficientTheta = coeffTheta; - } - - public void setSpeedCoefficients(double coeffX, double coeffY, double coeffTheta) { - speedCoefficientX = coeffX; - speedCoefficientY = coeffY; - speedCoefficientTheta = coeffTheta; - } - - public void setMaximums(double maxDistance, double maxSpeed, double maxAngleDegrees) { - maximumAllowedDistance = maxDistance; - maximumRobotSpeed = maxSpeed; - maximumViewingAngle = Math.toRadians(maxAngleDegrees); - } - /** * Helper class to store pose estimation results with confidence metrics. */ From 2cb1d8235510afc4bcc2e12939c2e5d552cdbb0a Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 14:42:29 -0700 Subject: [PATCH 15/43] corrected a comp --- src/main/java/frc/robot/subsystems/VisionSystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 9de9f0a7..ae0219db 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -69,7 +69,7 @@ public class VisionSystem extends SubsystemBase { // PhotonVision and odometry references private final PhotonCamera photonCamera; private final PhotonPoseEstimator poseEstimator; // MULTI_TAG_PNP_ON_COPROCESSOR - private final PhotonPoseEstimator trigsolvePoseEstimator; // LOWEST_AMBIGUITY with trigsolve + private final PhotonPoseEstimator trigsolvePoseEstimator; // PNP_DISTANCE_TRIG_SOLVE private PhotonPipelineResult latestVisionResult; private final BooleanSupplier isRedSide; private Pose2d lastKnownPose = new Pose2d(0, 0, new Rotation2d()); From 9e43cafccbe793660a0a99b8147333876989c1bc Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 15:10:11 -0700 Subject: [PATCH 16/43] fixed skew angle --- .../frc/robot/subsystems/VisionSystem.java | 33 +++++++++++-------- 1 file changed, 19 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index ae0219db..f53de69e 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -13,6 +13,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; @@ -168,28 +169,22 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean DogLog.log("Vision/DistanceFilter", true); return null; } - double averageYawDeg = + // Compute true viewing skew (how oblique the camera is to the tag face) + double averageSkewRad = validTags.stream() - .mapToDouble(PhotonTrackedTarget::getYaw) - .map(Math::abs) - .average() - .orElse(0.0); - double averagePitchDeg = - validTags.stream() - .mapToDouble(PhotonTrackedTarget::getPitch) - .map(Math::abs) + .mapToDouble(this::computeTargetSkewRad) .average() .orElse(0.0); - // Reject overly oblique views (>70 degrees in either axis) - if (averageYawDeg > 70.0 || averagePitchDeg > 70.0) { + // Reject overly oblique views (e.g., >70° combined yaw/pitch off-axis) + if (Math.toDegrees(averageSkewRad) > 70.0) { if (forceAdd) return null; - DogLog.log("Vision/ViewAngleTooLarge", true); + DogLog.log("Vision/ViewSkewTooLarge", true); return null; } - // retain previous semantics for noise calculation (uses yaw) - double averageAngle = Math.toRadians(averageYawDeg); + // Use true viewing skew (radians) for noise calculation + double averageAngle = averageSkewRad; double currentSpeed = Math.hypot( swerveDrive.getRobotSpeeds().vxMetersPerSecond, @@ -437,4 +432,14 @@ public PoseEstimateResult(Pose2d pose, double confidence, double averageDistance this.timestamp = timestamp; } } + /** + * Computes the camera's off-axis viewing skew for a single tag, in radians. + * Uses the rotation from camera -> target; pitch (Y) and yaw (Z) near zero imply head-on view. + */ + private double computeTargetSkewRad(PhotonTrackedTarget t) { + Rotation3d r = t.getBestCameraToTarget().getRotation(); + // r.getY() and r.getZ() are radians (pitch and yaw). + // Combined off-axis = sqrt(pitch^2 + yaw^2) + return Math.hypot(r.getY(), r.getZ()); + } } \ No newline at end of file From 7bf33d39148b9b76dc776a3b53e3e8933b795b45 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 15:17:17 -0700 Subject: [PATCH 17/43] normalized units & used correct distance measurement --- .../frc/robot/subsystems/VisionSystem.java | 51 +++++++++++++------ 1 file changed, 35 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index f53de69e..cedea99a 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -159,32 +159,47 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean // Compute effective metrics for solution int tagCount = validTags.size(); + // Use camera→target distance from PV (avoids odometry dependence) double averageDistance = validTags.stream() - .mapToDouble(t -> getDistanceToTag(t.getFiducialId())) + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) .average() .orElse(Double.NaN); - if (Double.isNaN(averageDistance) || averageDistance > maximumAllowedDistance) { + double minDistance = + validTags.stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .min() + .orElse(Double.NaN); + if (Double.isNaN(minDistance) || minDistance > maximumAllowedDistance) { if (forceAdd) return null; DogLog.log("Vision/DistanceFilter", true); return null; } // Compute true viewing skew (how oblique the camera is to the tag face) - double averageSkewRad = - validTags.stream() - .mapToDouble(this::computeTargetSkewRad) - .average() - .orElse(0.0); + double avgSkewRad = + validTags.stream().mapToDouble(this::computeTargetSkewRad).average().orElse(0.0); + double minSkewRad = + validTags.stream().mapToDouble(this::computeTargetSkewRad).min().orElse(0.0); + double maxSkewRad = + validTags.stream().mapToDouble(this::computeTargetSkewRad).max().orElse(0.0); - // Reject overly oblique views (e.g., >70° combined yaw/pitch off-axis) - if (Math.toDegrees(averageSkewRad) > 70.0) { - if (forceAdd) return null; - DogLog.log("Vision/ViewSkewTooLarge", true); - return null; + // Strategy-specific gating: trigsolve rejects only if ALL tags are too skewed; PnP rejects if ANY is too skewed + if (useTrigsolve) { + if (Math.toDegrees(minSkewRad) > 70.0) { + if (forceAdd) return null; + DogLog.log("Vision/ViewSkewTooLarge", true); + return null; + } + } else { + if (Math.toDegrees(maxSkewRad) > 70.0) { + if (forceAdd) return null; + DogLog.log("Vision/ViewSkewTooLarge", true); + return null; + } } - // Use true viewing skew (radians) for noise calculation - double averageAngle = averageSkewRad; + // Use strategy-appropriate skew for noise scaling + double averageAngle = useTrigsolve ? minSkewRad : avgSkewRad; double currentSpeed = Math.hypot( swerveDrive.getRobotSpeeds().vxMetersPerSecond, @@ -242,9 +257,13 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean averageAngle, currentSpeed, tagCount); - + // Calculate overall measurement quality (lower noise = higher confidence) - double totalNoise = Math.sqrt(noiseX * noiseX + noiseY * noiseY + noiseTheta * noiseTheta); + // Normalize axes to unitless before combining (meters/radians) + double nx = noiseX / Math.max(baseNoiseX, 1e-9); + double ny = noiseY / Math.max(baseNoiseY, 1e-9); + double nth = noiseTheta / Math.max(baseNoiseTheta, 1e-9); + double totalNoise = Math.sqrt(nx * nx + ny * ny + nth * nth); confidence = 1.0 / (1.0 + totalNoise); } From 6c2b4c634b5c88a947a951de98d2e3aa5412ef13 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 15:24:27 -0700 Subject: [PATCH 18/43] changed parent functions --- .../frc/robot/subsystems/VisionSystem.java | 35 +++++++++++++++---- 1 file changed, 28 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index cedea99a..fbc88678 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -417,15 +417,36 @@ private double computeMeasurementNoise( double angleRad, double robotSpeed, int tagCount) { - double tagCountScale = 1.0 / Math.sqrt(Math.max(tagCount, 1)); + // --- Tag count factor (diminishing returns). Keep same semantics but saturate at 4 tags. + // 1 tag = 1.0x, 2 tags ≈ 0.71x, 3 tags ≈ 0.58x, 4+ tags = 0.5x + int effectiveTags = Math.max(1, Math.min(tagCount, 4)); + double tagCountScale = 1.0 / Math.sqrt(effectiveTags); + + // --- Distance term (UNCHANGED as requested) double distanceTerm = baseNoise + distanceCoefficient * distance * distance; - double cosMax = Math.cos(maximumViewingAngle); - double normalizedAngle = 0.0; - if (1.0 - cosMax > 1e-9) { - normalizedAngle = (1.0 - Math.cos(angleRad)) / (1.0 - cosMax); - } + + // --- Angle factor: use foreshortening model ~ sec^2(theta). + // Normalize so angleTerm grows from 1 at 0° up to (1 + angleCoefficient) at maximumViewingAngle. + // Guard against cos(θ) → 0 near 90°. + double theta = Math.max(0.0, Math.min(angleRad, 0.999 * maximumViewingAngle)); + double cosT = Math.cos(theta); + double sec2T = 1.0 / Math.max(cosT * cosT, 1e-6); // sec^2(theta) + + double cosMax = Math.cos(0.999 * maximumViewingAngle); + double sec2Max = 1.0 / Math.max(cosMax * cosMax, 1e-6); + + // Map sec^2(theta) to [0,1] where 0 at head-on, 1 at max angle + double normalizedAngle = (sec2T - 1.0) / Math.max(sec2Max - 1.0, 1e-6); + normalizedAngle = Math.max(0.0, Math.min(normalizedAngle, 1.0)); + double angleTerm = 1.0 + angleCoefficient * normalizedAngle; - double speedTerm = 1.0 + speedCoefficient * (robotSpeed / maximumRobotSpeed); + + // --- Speed factor: quadratic with saturation at max speed + double v = Math.max(0.0, Math.min(robotSpeed, maximumRobotSpeed)); + double normalizedSpeed = (v / Math.max(maximumRobotSpeed, 1e-6)); + double speedTerm = 1.0 + speedCoefficient * (normalizedSpeed * normalizedSpeed); + + // Combine with calibration gain. Keep multiplicative structure. return calibrationFactor * tagCountScale * distanceTerm * angleTerm * speedTerm; } /** From 24dcd9d4f48276c8f5a856424249f6d9a7a5f88d Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 15:38:56 -0700 Subject: [PATCH 19/43] fixed distance scaling --- src/main/java/frc/robot/subsystems/VisionSystem.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index fbc88678..90d569cd 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -47,11 +47,11 @@ public class VisionSystem extends SubsystemBase { // Base noise tuning parameters (tweakable) private double calibrationFactor = 1.0; private double baseNoiseX = 0.01; // meters - private double baseNoiseY = 0.01; + private double baseNoiseY = 0.0; private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.0444898; // noise growth per meter - private double distanceCoefficientY = 0.0444898; + private double distanceCoefficientX = 0.019194; // noise growth per meter + private double distanceCoefficientY = 0.019194; private double distanceCoefficientTheta = 1; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle @@ -423,7 +423,7 @@ private double computeMeasurementNoise( double tagCountScale = 1.0 / Math.sqrt(effectiveTags); // --- Distance term (UNCHANGED as requested) - double distanceTerm = baseNoise + distanceCoefficient * distance * distance; + double distanceTerm = baseNoise + distanceCoefficient * distance * distance * distance; // --- Angle factor: use foreshortening model ~ sec^2(theta). // Normalize so angleTerm grows from 1 at 0° up to (1 + angleCoefficient) at maximumViewingAngle. From 6e60a87794717b045d421bf0762b21a78cb38331 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 15:43:44 -0700 Subject: [PATCH 20/43] quadratic 0.05 --- src/main/java/frc/robot/subsystems/VisionSystem.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 90d569cd..0d8c3819 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -50,8 +50,8 @@ public class VisionSystem extends SubsystemBase { private double baseNoiseY = 0.0; private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.019194; // noise growth per meter - private double distanceCoefficientY = 0.019194; + private double distanceCoefficientX = 0.05; // noise growth per m^2 + private double distanceCoefficientY = 0.05; private double distanceCoefficientTheta = 1; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle @@ -423,7 +423,7 @@ private double computeMeasurementNoise( double tagCountScale = 1.0 / Math.sqrt(effectiveTags); // --- Distance term (UNCHANGED as requested) - double distanceTerm = baseNoise + distanceCoefficient * distance * distance * distance; + double distanceTerm = baseNoise + distanceCoefficient * distance * distance; // --- Angle factor: use foreshortening model ~ sec^2(theta). // Normalize so angleTerm grows from 1 at 0° up to (1 + angleCoefficient) at maximumViewingAngle. From 7e61f55a309ffd053b6ce45476495b62e6f13b68 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 16 Aug 2025 15:46:32 -0700 Subject: [PATCH 21/43] vibe coded logging --- .../frc/robot/subsystems/VisionSystem.java | 280 +++++++++++------- 1 file changed, 167 insertions(+), 113 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 0d8c3819..b0f93167 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -50,7 +50,7 @@ public class VisionSystem extends SubsystemBase { private double baseNoiseY = 0.0; private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.05; // noise growth per m^2 + private double distanceCoefficientX = 0.05; // noise growth per meter private double distanceCoefficientY = 0.05; private double distanceCoefficientTheta = 1; @@ -140,11 +140,16 @@ public void addFilteredPose(boolean useTrigsolve) { * @return PoseEstimateResult containing the pose and confidence data, or null if failed */ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean forceAdd) { + final String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; if (latestVisionResult == null || !latestVisionResult.hasTargets()) { - if (forceAdd) return null; - DogLog.log("Vision/MeasurementUsed", false); + if (!forceAdd) { + v(strategyPrefix + "/HasTargets", false); + vts(strategyPrefix + "/Frame/NoTargets"); + } return null; } + v(strategyPrefix + "/HasTargets", true); + vts(strategyPrefix + "/Frame/Start"); // Filter tags to only those on the active side List validTags = @@ -156,9 +161,15 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean DogLog.log("Vision/TagFilter", false); return null; } + int tagCount = validTags.size(); + v(strategyPrefix + "/Tags/Count", tagCount); + String tagIdsCsv = validTags.stream() + .map(t -> Integer.toString(t.getFiducialId())) + .sorted() + .collect(Collectors.joining(",")); + v(strategyPrefix + "/Tags/IDs", tagIdsCsv); // Compute effective metrics for solution - int tagCount = validTags.size(); // Use camera→target distance from PV (avoids odometry dependence) double averageDistance = validTags.stream() @@ -170,9 +181,13 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) .min() .orElse(Double.NaN); + v(strategyPrefix + "/Range/AvgM", averageDistance); + v(strategyPrefix + "/Range/MinM", minDistance); if (Double.isNaN(minDistance) || minDistance > maximumAllowedDistance) { - if (forceAdd) return null; - DogLog.log("Vision/DistanceFilter", true); + if (!forceAdd) { + v(strategyPrefix + "/Gate/DistanceRejected", true); + v(strategyPrefix + "/Range/MaxAllowedM", maximumAllowedDistance); + } return null; } // Compute true viewing skew (how oblique the camera is to the tag face) @@ -183,17 +198,25 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean double maxSkewRad = validTags.stream().mapToDouble(this::computeTargetSkewRad).max().orElse(0.0); + v(strategyPrefix + "/Skew/AvgDeg", Math.toDegrees(avgSkewRad)); + v(strategyPrefix + "/Skew/MinDeg", Math.toDegrees(minSkewRad)); + v(strategyPrefix + "/Skew/MaxDeg", Math.toDegrees(maxSkewRad)); + // Strategy-specific gating: trigsolve rejects only if ALL tags are too skewed; PnP rejects if ANY is too skewed if (useTrigsolve) { if (Math.toDegrees(minSkewRad) > 70.0) { - if (forceAdd) return null; - DogLog.log("Vision/ViewSkewTooLarge", true); + if (!forceAdd) { + v(strategyPrefix + "/Gate/SkewRejected", true); + v(strategyPrefix + "/Skew/CutoffDeg", 70.0); + } return null; } } else { if (Math.toDegrees(maxSkewRad) > 70.0) { - if (forceAdd) return null; - DogLog.log("Vision/ViewSkewTooLarge", true); + if (!forceAdd) { + v(strategyPrefix + "/Gate/SkewRejected", true); + v(strategyPrefix + "/Skew/CutoffDeg", 70.0); + } return null; } } @@ -204,6 +227,8 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean Math.hypot( swerveDrive.getRobotSpeeds().vxMetersPerSecond, swerveDrive.getRobotSpeeds().vyMetersPerSecond); + v(strategyPrefix + "/Inputs/SkewUsedDeg", Math.toDegrees(averageAngle)); + v(strategyPrefix + "/Inputs/SpeedMps", currentSpeed); // Choose the appropriate pose estimator and reference pose PhotonPoseEstimator selectedEstimator = useTrigsolve ? trigsolvePoseEstimator : poseEstimator; @@ -213,9 +238,10 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean selectedEstimator.setReferencePose(referencePos); Optional maybePose = selectedEstimator.update(latestVisionResult); if (maybePose.isEmpty()) { - if (forceAdd) return null; - String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; - DogLog.log("Vision/" + strategyPrefix + "PoseEstimateFailed", true); + if (!forceAdd) { + v(strategyPrefix + "/Pose/EstimateFailed", true); + vts(strategyPrefix + "/Frame/End"); + } return null; } @@ -227,44 +253,42 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean if (useTrigsolve) { // For trigsolve, calculate confidence based on the measurement noise multipliers // Lower total noise = higher confidence - double noiseX = - computeMeasurementNoise( - baseNoiseX, - distanceCoefficientX, - angleCoefficientX, - speedCoefficientX, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - double noiseY = - computeMeasurementNoise( - baseNoiseY, - distanceCoefficientY, - angleCoefficientY, - speedCoefficientY, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - double noiseTheta = - computeMeasurementNoise( - baseNoiseTheta, - distanceCoefficientTheta, - angleCoefficientTheta, - speedCoefficientTheta, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - - // Calculate overall measurement quality (lower noise = higher confidence) - // Normalize axes to unitless before combining (meters/radians) - double nx = noiseX / Math.max(baseNoiseX, 1e-9); - double ny = noiseY / Math.max(baseNoiseY, 1e-9); - double nth = noiseTheta / Math.max(baseNoiseTheta, 1e-9); - double totalNoise = Math.sqrt(nx * nx + ny * ny + nth * nth); + NoiseComponents nxC = computeNoiseComponents( + baseNoiseX, distanceCoefficientX, angleCoefficientX, speedCoefficientX, + averageDistance, averageAngle, currentSpeed, tagCount); + NoiseComponents nyC = computeNoiseComponents( + baseNoiseY, distanceCoefficientY, angleCoefficientY, speedCoefficientY, + averageDistance, averageAngle, currentSpeed, tagCount); + NoiseComponents nthC = computeNoiseComponents( + baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, speedCoefficientTheta, + averageDistance, averageAngle, currentSpeed, tagCount); + + double noiseX = nxC.total; + double noiseY = nyC.total; + double noiseTheta = nthC.total; + + // Unitless blend for confidence + double nxU = noiseX / Math.max(baseNoiseX, 1e-9); + double nyU = noiseY / Math.max(baseNoiseY, 1e-9); + double nthU = noiseTheta / Math.max(baseNoiseTheta, 1e-9); + double totalNoise = Math.sqrt(nxU * nxU + nyU * nyU + nthU * nthU); confidence = 1.0 / (1.0 + totalNoise); + + // Structured logs for tuning (Trigsolve only) + v(strategyPrefix + "/Confidence", confidence); + v(strategyPrefix + "/Noise/X/Total", noiseX); + v(strategyPrefix + "/Noise/Y/Total", noiseY); + v(strategyPrefix + "/Noise/Theta/Total", noiseTheta); + v(strategyPrefix + "/Noise/TagScale", nxC.tagScale); // same for all axes + v(strategyPrefix + "/Noise/X/DistanceTerm", nxC.distanceTerm); + v(strategyPrefix + "/Noise/X/AngleTerm", nxC.angleTerm); + v(strategyPrefix + "/Noise/X/SpeedTerm", nxC.speedTerm); + v(strategyPrefix + "/Noise/Y/DistanceTerm", nyC.distanceTerm); + v(strategyPrefix + "/Noise/Y/AngleTerm", nyC.angleTerm); + v(strategyPrefix + "/Noise/Y/SpeedTerm", nyC.speedTerm); + v(strategyPrefix + "/Noise/Theta/DistanceTerm", nthC.distanceTerm); + v(strategyPrefix + "/Noise/Theta/AngleTerm", nthC.angleTerm); + v(strategyPrefix + "/Noise/Theta/SpeedTerm", nthC.speedTerm); } // If this is just for confidence comparison, return the result without adding to odometry @@ -316,9 +340,9 @@ private PoseEstimateResult processTrigsolveWithConfidenceCheck(Pose2d measuredPo currentSpeed, tagCount, timestamp); } else { // Another camera had better confidence, don't add measurement - DogLog.log("Vision/TrigsolveLowerConfidence", true); - DogLog.log("Vision/TrigsolveOurConfidence", confidence); - DogLog.log("Vision/TrigsolveBestConfidence", bestConfidence); + v("Trigsolve/Confidence/LowerThanOtherCam", true); + v("Trigsolve/Confidence/Ours", confidence); + v("Trigsolve/Confidence/Best", bestConfidence); return null; } } @@ -329,62 +353,61 @@ private PoseEstimateResult processTrigsolveWithConfidenceCheck(Pose2d measuredPo private PoseEstimateResult processPoseEstimate(Pose2d measuredPose, boolean useTrigsolve, double averageDistance, double averageAngle, double currentSpeed, int tagCount, double timestamp) { - // Compute measurement noise for each axis - double noiseX = - computeMeasurementNoise( - baseNoiseX, - distanceCoefficientX, - angleCoefficientX, - speedCoefficientX, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - double noiseY = - computeMeasurementNoise( - baseNoiseY, - distanceCoefficientY, - angleCoefficientY, - speedCoefficientY, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - double noiseTheta = - computeMeasurementNoise( - baseNoiseTheta, - distanceCoefficientTheta, - angleCoefficientTheta, - speedCoefficientTheta, - averageDistance, - averageAngle, - currentSpeed, - tagCount); + // Compute measurement noise for each axis and log components + NoiseComponents nxC = computeNoiseComponents( + baseNoiseX, distanceCoefficientX, angleCoefficientX, speedCoefficientX, + averageDistance, averageAngle, currentSpeed, tagCount); + NoiseComponents nyC = computeNoiseComponents( + baseNoiseY, distanceCoefficientY, angleCoefficientY, speedCoefficientY, + averageDistance, averageAngle, currentSpeed, tagCount); + NoiseComponents nthC = computeNoiseComponents( + baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, speedCoefficientTheta, + averageDistance, averageAngle, currentSpeed, tagCount); + + double noiseX = nxC.total; + double noiseY = nyC.total; + double noiseTheta = nthC.total; String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; - DogLog.log("Vision/" + strategyPrefix + "NoiseX", noiseX); - DogLog.log("Vision/" + strategyPrefix + "NoiseY", noiseY); - DogLog.log("Vision/" + strategyPrefix + "NoiseTheta", noiseTheta); - + v(strategyPrefix + "/Noise/X/Total", noiseX); + v(strategyPrefix + "/Noise/Y/Total", noiseY); + v(strategyPrefix + "/Noise/Theta/Total", noiseTheta); + v(strategyPrefix + "/Noise/TagScale", nxC.tagScale); + v(strategyPrefix + "/Noise/X/DistanceTerm", nxC.distanceTerm); + v(strategyPrefix + "/Noise/X/AngleTerm", nxC.angleTerm); + v(strategyPrefix + "/Noise/X/SpeedTerm", nxC.speedTerm); + v(strategyPrefix + "/Noise/Y/DistanceTerm", nyC.distanceTerm); + v(strategyPrefix + "/Noise/Y/AngleTerm", nyC.angleTerm); + v(strategyPrefix + "/Noise/Y/SpeedTerm", nyC.speedTerm); + v(strategyPrefix + "/Noise/Theta/DistanceTerm", nthC.distanceTerm); + v(strategyPrefix + "/Noise/Theta/AngleTerm", nthC.angleTerm); + v(strategyPrefix + "/Noise/Theta/SpeedTerm", nthC.speedTerm); + // Update the appropriate reference pose if (useTrigsolve) { lastKnownTrigsolvePose = measuredPose; } else { lastKnownPose = measuredPose; } - - DogLog.log("Vision/" + cameraId.toString() + strategyPrefix + "Pose", measuredPose); - + // Choose timestamp: use vision timestamp unless it differs too much from FPGA double fpgaTimestamp = Timer.getFPGATimestamp(); double timestampDifference = Math.abs(timestamp - fpgaTimestamp); double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp : timestamp; + v(strategyPrefix + "/Pose/X", measuredPose.getX()); + v(strategyPrefix + "/Pose/Y", measuredPose.getY()); + v(strategyPrefix + "/Pose/ThetaDeg", measuredPose.getRotation().getDegrees()); + v(strategyPrefix + "/Timestamp/Vision", timestamp); + v(strategyPrefix + "/Timestamp/FPGA", fpgaTimestamp); + v(strategyPrefix + "/Timestamp/Delta", timestampDifference); + // Build the noise vector and add the vision measurement Matrix noiseVector = VecBuilder.fill(noiseX, noiseY, noiseTheta); swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); - DogLog.log("Vision/" + strategyPrefix + "MeasurementUsed", true); - + v(strategyPrefix + "/MeasurementUsed", true); + vts(strategyPrefix + "/Frame/End"); + return new PoseEstimateResult(measuredPose, 1.0, averageDistance, averageAngle, currentSpeed, tagCount, timestamp); } @@ -408,7 +431,22 @@ private double getDistanceToTag(int tagId) { .orElse(Double.NaN); } - private double computeMeasurementNoise( + // ─── Vision logging helpers ────────────────────────────────────────────────── + private String vKey(String subkey) { return "Vision/" + cameraId.toString() + "/" + subkey; } + private void vts(String subkey) { DogLog.timestamp(vKey(subkey)); } + private void v(String subkey, double val) { DogLog.log(vKey(subkey), val); } + private void v(String subkey, boolean val) { DogLog.log(vKey(subkey), val); } + private void v(String subkey, String val) { DogLog.log(vKey(subkey), val); } + + private static class NoiseComponents { + double tagScale; // 1/sqrt(tags) + double distanceTerm; // base + k*d^2 + double angleTerm; // normalized sec^2 + double speedTerm; // 1 + k*(v/vmax)^2 + double total; // calibrationFactor * tagScale * distanceTerm * angleTerm * speedTerm + } + + private NoiseComponents computeNoiseComponents( double baseNoise, double distanceCoefficient, double angleCoefficient, @@ -417,37 +455,53 @@ private double computeMeasurementNoise( double angleRad, double robotSpeed, int tagCount) { - // --- Tag count factor (diminishing returns). Keep same semantics but saturate at 4 tags. - // 1 tag = 1.0x, 2 tags ≈ 0.71x, 3 tags ≈ 0.58x, 4+ tags = 0.5x + NoiseComponents c = new NoiseComponents(); + + // Tag count factor (diminishing returns; cap at 4) int effectiveTags = Math.max(1, Math.min(tagCount, 4)); - double tagCountScale = 1.0 / Math.sqrt(effectiveTags); + c.tagScale = 1.0 / Math.sqrt(effectiveTags); - // --- Distance term (UNCHANGED as requested) - double distanceTerm = baseNoise + distanceCoefficient * distance * distance; + // Distance term (keep as d^2 per your model) + c.distanceTerm = baseNoise + distanceCoefficient * distance * distance; - // --- Angle factor: use foreshortening model ~ sec^2(theta). - // Normalize so angleTerm grows from 1 at 0° up to (1 + angleCoefficient) at maximumViewingAngle. - // Guard against cos(θ) → 0 near 90°. + // Angle term (foreshortening ~ sec^2(theta), normalized to [1, 1+coef]) double theta = Math.max(0.0, Math.min(angleRad, 0.999 * maximumViewingAngle)); double cosT = Math.cos(theta); - double sec2T = 1.0 / Math.max(cosT * cosT, 1e-6); // sec^2(theta) - + double sec2T = 1.0 / Math.max(cosT * cosT, 1e-6); double cosMax = Math.cos(0.999 * maximumViewingAngle); double sec2Max = 1.0 / Math.max(cosMax * cosMax, 1e-6); - - // Map sec^2(theta) to [0,1] where 0 at head-on, 1 at max angle double normalizedAngle = (sec2T - 1.0) / Math.max(sec2Max - 1.0, 1e-6); normalizedAngle = Math.max(0.0, Math.min(normalizedAngle, 1.0)); + c.angleTerm = 1.0 + angleCoefficient * normalizedAngle; - double angleTerm = 1.0 + angleCoefficient * normalizedAngle; + // Speed term (quadratic, saturated) + double vNorm = Math.max(0.0, Math.min(robotSpeed, maximumRobotSpeed)) + / Math.max(maximumRobotSpeed, 1e-6); + c.speedTerm = 1.0 + speedCoefficient * (vNorm * vNorm); - // --- Speed factor: quadratic with saturation at max speed - double v = Math.max(0.0, Math.min(robotSpeed, maximumRobotSpeed)); - double normalizedSpeed = (v / Math.max(maximumRobotSpeed, 1e-6)); - double speedTerm = 1.0 + speedCoefficient * (normalizedSpeed * normalizedSpeed); + c.total = calibrationFactor * c.tagScale * c.distanceTerm * c.angleTerm * c.speedTerm; + return c; + } - // Combine with calibration gain. Keep multiplicative structure. - return calibrationFactor * tagCountScale * distanceTerm * angleTerm * speedTerm; + private double computeMeasurementNoise( + double baseNoise, + double distanceCoefficient, + double angleCoefficient, + double speedCoefficient, + double distance, + double angleRad, + double robotSpeed, + int tagCount) { + NoiseComponents c = computeNoiseComponents( + baseNoise, + distanceCoefficient, + angleCoefficient, + speedCoefficient, + distance, + angleRad, + robotSpeed, + tagCount); + return c.total; } /** * Helper class to store pose estimation results with confidence metrics. From df6e226a28d63e5c5d26262baef469f6e3bf9b37 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Mon, 25 Aug 2025 18:53:40 -0700 Subject: [PATCH 22/43] vision commit on monday --- src/main/java/frc/robot/Robot.java | 4 +- .../frc/robot/subsystems/VisionSystem.java | 333 +++++++++++------- 2 files changed, 207 insertions(+), 130 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 221eb58e..66dca234 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -71,8 +71,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); m_robotContainer.doTelemetry(); - visionRight.addFilteredPose(true); - visionLeft.addFilteredPose(true); + visionRight.addFilteredPose(false); + visionLeft.addFilteredPose(false); DogLog.log("KalmanDebug/drivetrainPose", driveTrain.getPose()); diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index b0f93167..80e4d297 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -47,11 +47,11 @@ public class VisionSystem extends SubsystemBase { // Base noise tuning parameters (tweakable) private double calibrationFactor = 1.0; private double baseNoiseX = 0.01; // meters - private double baseNoiseY = 0.0; + private double baseNoiseY = 0.01; private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.05; // noise growth per meter - private double distanceCoefficientY = 0.05; + private double distanceCoefficientX = 0.055; // noise growth per meter + private double distanceCoefficientY = 0.055; private double distanceCoefficientTheta = 1; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle @@ -75,9 +75,8 @@ public class VisionSystem extends SubsystemBase { private final BooleanSupplier isRedSide; private Pose2d lastKnownPose = new Pose2d(0, 0, new Rotation2d()); private Pose2d lastKnownTrigsolvePose = new Pose2d(0, 0, new Rotation2d()); - private final SwerveSubsystem swerveDrive = SwerveSubsystem.getInstance(); - private final AprilTagFieldLayout fieldLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + private SwerveSubsystem swerveDrive; + private final AprilTagFieldLayout fieldLayout; public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { this.isRedSide = isRedSide; @@ -85,16 +84,35 @@ public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide photonCamera = new PhotonCamera(cameraId.toString()); Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); - // Initialize both pose estimators - poseEstimator = - new PhotonPoseEstimator( - fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + // Initialize field layout with error handling + AprilTagFieldLayout tempLayout = null; + try { + tempLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + } catch (Exception e) { + System.err.println("Failed to load AprilTag field layout: " + e.getMessage()); + e.printStackTrace(); + } + this.fieldLayout = tempLayout; - trigsolvePoseEstimator = - new PhotonPoseEstimator( - fieldLayout, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, cameraToRobot); + // Initialize both pose estimators only if field layout loaded successfully + if (fieldLayout != null) { + poseEstimator = + new PhotonPoseEstimator( + fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + + trigsolvePoseEstimator = + new PhotonPoseEstimator( + fieldLayout, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, cameraToRobot); + } else { + poseEstimator = null; + trigsolvePoseEstimator = null; + } latestVisionResult = null; + + // Minimal debug logging (guarded by ENABLE_LOGS) + v("Init", true); + v("FieldLayoutLoaded", fieldLayout != null); } public static VisionSystem getInstance( @@ -108,8 +126,50 @@ public static VisionSystem getInstance( @Override public void periodic() { - for (var x : photonCamera.getAllUnreadResults()) { - latestVisionResult = x; + // Initialize swerve drive if not already done + if (swerveDrive == null) { + try { + swerveDrive = SwerveSubsystem.getInstance(); + } catch (Exception e) { + v("SwerveNotReady", true); + return; + } + } + + // Log that periodic is running (guarded) + v("Periodic", true); + + // Check camera connection + boolean cameraConnected = photonCamera.isConnected(); + v("CameraConnected", cameraConnected); + + if (!cameraConnected) { + v("CameraDisconnected", true); + return; + } + + // Get all unread results + var results = photonCamera.getAllUnreadResults(); + v("UnreadResultsCount", results.size()); + + for (var result : results) { + latestVisionResult = result; + v("ResultTimestamp", result.getTimestampSeconds()); + v("HasTargets", result.hasTargets()); + if (result.hasTargets()) { + v("TargetCount", result.getTargets().size()); + } + } + + // If we have a recent result, try to add it + if (latestVisionResult != null) { + double timeSinceResult = Timer.getFPGATimestamp() - latestVisionResult.getTimestampSeconds(); + v("TimeSinceLastResult", timeSinceResult); + + // Only use results that are recent (within 0.5 seconds) + if (timeSinceResult < 0.5) { + addFilteredPose(); + } } } @@ -125,7 +185,7 @@ public void addFilteredPose() { * Attempts to fuse a vision measurement into the swerve pose estimator, dropping readings that * fail validity checks, and computing noise dynamically via computeMeasurementNoise(). * - * @param useTrigsolve If true, uses the trigsolve pose estimator (PNP_DISTANCE_TRIG_SOLVE strategy). + * @param useTrigsolve If true, uses the trigsolve pose estimator (LOWEST_AMBIGUITY strategy). * If false, uses the multi-tag PnP pose estimator (MULTI_TAG_PNP_ON_COPROCESSOR strategy). */ public void addFilteredPose(boolean useTrigsolve) { @@ -141,6 +201,24 @@ public void addFilteredPose(boolean useTrigsolve) { */ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean forceAdd) { final String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; + + // Check prerequisites + if (fieldLayout == null) { + v(strategyPrefix + "/FieldLayoutMissing", true); + return null; + } + + if (swerveDrive == null) { + v(strategyPrefix + "/SwerveDriveNull", true); + return null; + } + + PhotonPoseEstimator selectedEstimator = useTrigsolve ? trigsolvePoseEstimator : poseEstimator; + if (selectedEstimator == null) { + v(strategyPrefix + "/PoseEstimatorNull", true); + return null; + } + if (latestVisionResult == null || !latestVisionResult.hasTargets()) { if (!forceAdd) { v(strategyPrefix + "/HasTargets", false); @@ -156,11 +234,21 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean latestVisionResult.getTargets().stream() .filter(t -> isTagOnActiveSide(t.getFiducialId())) .collect(Collectors.toList()); + + // Log all detected tags for debugging + String allTagIds = latestVisionResult.getTargets().stream() + .map(t -> Integer.toString(t.getFiducialId())) + .collect(Collectors.joining(",")); + v(strategyPrefix + "/AllDetectedTags", allTagIds); + v(strategyPrefix + "/IsRedSide", isRedSide.getAsBoolean()); + if (validTags.isEmpty()) { - if (forceAdd) return null; - DogLog.log("Vision/TagFilter", false); + if (!forceAdd) { + v(strategyPrefix + "/TagFilter", false); + } return null; } + int tagCount = validTags.size(); v(strategyPrefix + "/Tags/Count", tagCount); String tagIdsCsv = validTags.stream() @@ -190,52 +278,54 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean } return null; } - // Compute true viewing skew (how oblique the camera is to the tag face) - double avgSkewRad = - validTags.stream().mapToDouble(this::computeTargetSkewRad).average().orElse(0.0); + // Compute true 3D viewing skew for each tag: angle between camera +X and NEGATED tag +Z double minSkewRad = validTags.stream().mapToDouble(this::computeTargetSkewRad).min().orElse(0.0); double maxSkewRad = validTags.stream().mapToDouble(this::computeTargetSkewRad).max().orElse(0.0); + double rmsSkewRad = computeWeightedRmsSkewRad(validTags); + double avgSkewRad = + validTags.stream().mapToDouble(this::computeTargetSkewRad).average().orElse(0.0); - v(strategyPrefix + "/Skew/AvgDeg", Math.toDegrees(avgSkewRad)); v(strategyPrefix + "/Skew/MinDeg", Math.toDegrees(minSkewRad)); + v(strategyPrefix + "/Skew/AvgDeg", Math.toDegrees(avgSkewRad)); + v(strategyPrefix + "/Skew/RmsDeg", Math.toDegrees(rmsSkewRad)); v(strategyPrefix + "/Skew/MaxDeg", Math.toDegrees(maxSkewRad)); - // Strategy-specific gating: trigsolve rejects only if ALL tags are too skewed; PnP rejects if ANY is too skewed - if (useTrigsolve) { - if (Math.toDegrees(minSkewRad) > 70.0) { - if (!forceAdd) { - v(strategyPrefix + "/Gate/SkewRejected", true); - v(strategyPrefix + "/Skew/CutoffDeg", 70.0); - } - return null; - } - } else { - if (Math.toDegrees(maxSkewRad) > 70.0) { - if (!forceAdd) { - v(strategyPrefix + "/Gate/SkewRejected", true); - v(strategyPrefix + "/Skew/CutoffDeg", 70.0); - } - return null; - } +// No skew gating: skew only affects noise scaling below. + +// Use strategy-appropriate skew for noise scaling +double averageAngle = useTrigsolve ? minSkewRad : rmsSkewRad; + double currentSpeed = 0.0; + try { + currentSpeed = Math.hypot( + swerveDrive.getRobotSpeeds().vxMetersPerSecond, + swerveDrive.getRobotSpeeds().vyMetersPerSecond); + } catch (Exception e) { + v(strategyPrefix + "/SpeedCalculationError", true); } - - // Use strategy-appropriate skew for noise scaling - double averageAngle = useTrigsolve ? minSkewRad : avgSkewRad; - double currentSpeed = - Math.hypot( - swerveDrive.getRobotSpeeds().vxMetersPerSecond, - swerveDrive.getRobotSpeeds().vyMetersPerSecond); + v(strategyPrefix + "/Inputs/SkewUsedDeg", Math.toDegrees(averageAngle)); v(strategyPrefix + "/Inputs/SpeedMps", currentSpeed); // Choose the appropriate pose estimator and reference pose - PhotonPoseEstimator selectedEstimator = useTrigsolve ? trigsolvePoseEstimator : poseEstimator; Pose2d referencePos = useTrigsolve ? lastKnownTrigsolvePose : lastKnownPose; + // Update reference pose with current odometry if we have it + try { + Pose2d currentOdometry = swerveDrive.getPose(); + if (currentOdometry != null) { + selectedEstimator.setReferencePose(currentOdometry); + v(strategyPrefix + "/ReferencePose/Pose", currentOdometry); + } else { + selectedEstimator.setReferencePose(referencePos); + } + } catch (Exception e) { + v(strategyPrefix + "/ReferencePoseError", true); + selectedEstimator.setReferencePose(referencePos); + } + // Get the pose from PhotonVision - selectedEstimator.setReferencePose(referencePos); Optional maybePose = selectedEstimator.update(latestVisionResult); if (maybePose.isEmpty()) { if (!forceAdd) { @@ -293,59 +383,16 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean // If this is just for confidence comparison, return the result without adding to odometry if (forceAdd) { - return new PoseEstimateResult(measuredPose, confidence, averageDistance, averageAngle, + return new PoseEstimateResult(measuredPose, confidence, averageDistance, averageAngle, currentSpeed, tagCount, latestVisionResult.getTimestampSeconds()); } - // For non-trigsolve or when not doing confidence comparison, proceed normally - if (!useTrigsolve) { - return processPoseEstimate(measuredPose, useTrigsolve, averageDistance, averageAngle, - currentSpeed, tagCount, latestVisionResult.getTimestampSeconds()); - } - - // For trigsolve, check confidence against other cameras - return processTrigsolveWithConfidenceCheck(measuredPose, confidence, averageDistance, - averageAngle, currentSpeed, tagCount, - latestVisionResult.getTimestampSeconds()); + // Process locally (no cross-camera comparison) + return processPoseEstimate(measuredPose, useTrigsolve, averageDistance, averageAngle, + currentSpeed, tagCount, latestVisionResult.getTimestampSeconds()); } - /** - * Processes trigsolve pose estimation with confidence checking across all cameras. - */ - private PoseEstimateResult processTrigsolveWithConfidenceCheck(Pose2d measuredPose, double confidence, - double averageDistance, double averageAngle, double currentSpeed, int tagCount, double timestamp) { - - // Get results from all other cameras for confidence comparison - double bestConfidence = confidence; - PoseEstimateResult bestResult = new PoseEstimateResult(measuredPose, confidence, averageDistance, - averageAngle, currentSpeed, tagCount, timestamp); - - // Check all other camera instances - for (Constants.Vision.Cameras otherCamera : Constants.Vision.Cameras.values()) { - if (otherCamera == this.cameraId) continue; // Skip self - - VisionSystem otherSystem = systemList[otherCamera.ordinal()]; - if (otherSystem != null) { - PoseEstimateResult otherResult = otherSystem.addFilteredPoseInternal(true, true); - if (otherResult != null && otherResult.confidence > bestConfidence) { - bestConfidence = otherResult.confidence; - bestResult = otherResult; - } - } - } - - // Only proceed if this camera has the best confidence - if (bestResult.pose == measuredPose) { // This camera won - return processPoseEstimate(measuredPose, true, averageDistance, averageAngle, - currentSpeed, tagCount, timestamp); - } else { - // Another camera had better confidence, don't add measurement - v("Trigsolve/Confidence/LowerThanOtherCam", true); - v("Trigsolve/Confidence/Ours", confidence); - v("Trigsolve/Confidence/Best", bestConfidence); - return null; - } - } + // (processTrigsolveWithConfidenceCheck removed) /** * Final processing and addition of pose estimate to odometry. @@ -395,20 +442,25 @@ private PoseEstimateResult processPoseEstimate(Pose2d measuredPose, boolean useT double timestampDifference = Math.abs(timestamp - fpgaTimestamp); double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp : timestamp; - v(strategyPrefix + "/Pose/X", measuredPose.getX()); - v(strategyPrefix + "/Pose/Y", measuredPose.getY()); - v(strategyPrefix + "/Pose/ThetaDeg", measuredPose.getRotation().getDegrees()); + v(strategyPrefix + "/Pose/Pose", measuredPose); v(strategyPrefix + "/Timestamp/Vision", timestamp); v(strategyPrefix + "/Timestamp/FPGA", fpgaTimestamp); v(strategyPrefix + "/Timestamp/Delta", timestampDifference); // Build the noise vector and add the vision measurement Matrix noiseVector = VecBuilder.fill(noiseX, noiseY, noiseTheta); - swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); - v(strategyPrefix + "/MeasurementUsed", true); + + try { + swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); + v(strategyPrefix + "/MeasurementUsed", true); + } catch (Exception e) { + v(strategyPrefix + "/MeasurementFailed", true); + v("AddMeasurementError", e.getMessage()); + } + vts(strategyPrefix + "/Frame/End"); - return new PoseEstimateResult(measuredPose, 1.0, averageDistance, averageAngle, + return new PoseEstimateResult(measuredPose, 1.0, averageDistance, averageAngle, currentSpeed, tagCount, timestamp); } @@ -418,30 +470,21 @@ private boolean isTagOnActiveSide(int tagId) { : BLUE_SIDE_TAG_IDS.contains(tagId); } - private double getDistanceToTag(int tagId) { - return fieldLayout - .getTagPose(tagId) - .map( - pose3d -> - pose3d - .getTranslation() - .getDistance( - new Translation3d( - swerveDrive.getPose().getX(), swerveDrive.getPose().getY(), 0.0))) - .orElse(Double.NaN); - } + // (getDistanceToTag removed) // ─── Vision logging helpers ────────────────────────────────────────────────── + private static final boolean ENABLE_LOGS = true; // flip true when tuning private String vKey(String subkey) { return "Vision/" + cameraId.toString() + "/" + subkey; } - private void vts(String subkey) { DogLog.timestamp(vKey(subkey)); } - private void v(String subkey, double val) { DogLog.log(vKey(subkey), val); } - private void v(String subkey, boolean val) { DogLog.log(vKey(subkey), val); } - private void v(String subkey, String val) { DogLog.log(vKey(subkey), val); } + private void vts(String subkey) { if (!ENABLE_LOGS) return; DogLog.timestamp(vKey(subkey)); } + private void v(String subkey, double val) { if (!ENABLE_LOGS) return; DogLog.log(vKey(subkey), val); } + private void v(String subkey, boolean val) { if (!ENABLE_LOGS) return; DogLog.log(vKey(subkey), val); } + private void v(String subkey, String val) { if (!ENABLE_LOGS) return; DogLog.log(vKey(subkey), val); } + private void v(String subkey, Pose2d val) { if (!ENABLE_LOGS) return; DogLog.log(vKey(subkey), val); } private static class NoiseComponents { double tagScale; // 1/sqrt(tags) double distanceTerm; // base + k*d^2 - double angleTerm; // normalized sec^2 + double angleTerm; // cosine-based (1 - cos θ) normalized double speedTerm; // 1 + k*(v/vmax)^2 double total; // calibrationFactor * tagScale * distanceTerm * angleTerm * speedTerm } @@ -464,13 +507,12 @@ private NoiseComponents computeNoiseComponents( // Distance term (keep as d^2 per your model) c.distanceTerm = baseNoise + distanceCoefficient * distance * distance; - // Angle term (foreshortening ~ sec^2(theta), normalized to [1, 1+coef]) + // Angle term (cosine-based): scale with 1 - cos(theta), normalized to cutoff double theta = Math.max(0.0, Math.min(angleRad, 0.999 * maximumViewingAngle)); double cosT = Math.cos(theta); - double sec2T = 1.0 / Math.max(cosT * cosT, 1e-6); double cosMax = Math.cos(0.999 * maximumViewingAngle); - double sec2Max = 1.0 / Math.max(cosMax * cosMax, 1e-6); - double normalizedAngle = (sec2T - 1.0) / Math.max(sec2Max - 1.0, 1e-6); + double denom = Math.max(1.0 - cosMax, 1e-6); + double normalizedAngle = (1.0 - cosT) / denom; // 0 at head-on, 1 at max angle normalizedAngle = Math.max(0.0, Math.min(normalizedAngle, 1.0)); c.angleTerm = 1.0 + angleCoefficient * normalizedAngle; @@ -503,6 +545,7 @@ private double computeMeasurementNoise( tagCount); return c.total; } + /** * Helper class to store pose estimation results with confidence metrics. */ @@ -526,14 +569,48 @@ public PoseEstimateResult(Pose2d pose, double confidence, double averageDistance this.timestamp = timestamp; } } + /** - * Computes the camera's off-axis viewing skew for a single tag, in radians. - * Uses the rotation from camera -> target; pitch (Y) and yaw (Z) near zero imply head-on view. + * Computes the true 3D off-axis viewing angle (skew) for a tag. + * Returns angle in radians: 0 = head-on, increases with obliqueness. + * Geometric skew: ACUTE angle between camera +X and tag face normal (−Z_tag). */ private double computeTargetSkewRad(PhotonTrackedTarget t) { + // Geometric skew: ACUTE angle between camera +X and tag face normal (−Z_tag). + // Use inverse rotation to express tag +Z in the camera frame. Rotation3d r = t.getBestCameraToTarget().getRotation(); - // r.getY() and r.getZ() are radians (pitch and yaw). - // Combined off-axis = sqrt(pitch^2 + yaw^2) - return Math.hypot(r.getY(), r.getZ()); + Translation3d nCam = new Translation3d(0.0, 0.0, 1.0).rotateBy(r.unaryMinus()); + + // Flip so the normal points into the scene, then take the acute angle (abs dot) vs +X. + double nx = -nCam.getX(); + double ny = -nCam.getY(); + double nz = -nCam.getZ(); + + double nNorm = Math.sqrt(nx * nx + ny * ny + nz * nz); + if (nNorm < 1e-9) return 0.0; + + // Absolute dot to make it invariant to 180° normal flips; yields [0, 90°]. + double cosAng = Math.abs(nx) / nNorm; // camera forward is (1,0,0) + cosAng = Math.max(-1.0, Math.min(1.0, cosAng)); + return Math.acos(cosAng); + } + + /** + * Weighted RMS of per-tag skew angles (radians) using geometric skew. + * Weights favor closer and front-on tags: + * w_i = cos^2(skew_i) / (d_i^2 + eps) + */ + private double computeWeightedRmsSkewRad(List tags) { + double num = 0.0, den = 0.0; + for (var t : tags) { + double s = computeTargetSkewRad(t); // radians from geometry + double d = t.getBestCameraToTarget().getTranslation().getNorm(); + double c = Math.cos(Math.min(s, 0.999 * maximumViewingAngle)); + double w = (c * c) / (d * d + 1e-6); + num += w * s * s; + den += w; + } + if (den <= 1e-9) return 0.0; + return Math.sqrt(num / den); } } \ No newline at end of file From d4d705ee2d043946a2cce2b521c8e987944d6377 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 30 Aug 2025 16:27:03 -0700 Subject: [PATCH 23/43] one commit BOSS --- .../frc/robot/subsystems/AnthonyVision.java | 266 ++++++++++++++++++ 1 file changed, 266 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/AnthonyVision.java diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java new file mode 100644 index 00000000..7ac41214 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -0,0 +1,266 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import dev.doglog.DogLog; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import java.util.List; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.stream.Collectors; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +/** + * VisionSystem fuses multi-tag AprilTag measurements from PhotonVision with swerve odometry by + * dynamically computing measurement noise based on tag count, distance, viewing angle, and robot + * speed. + */ +public class AnthonyVision extends SubsystemBase { + // static member of AnthonyVision that contains array of all existing AnthonyVision systems + private static AnthonyVision[] systemList = + new AnthonyVision[Constants.Vision.Cameras.values().length]; + + // Data type is "Cameras", an enum defined in Constants.java with only two options (left, right) + private final Constants.Vision.Cameras cameraId; + + // Reef tag IDs for each side of the field + private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); + private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); + + // Noise parameters + private double calibrationFactor = 1.0; // constant multiplier to everything + private double baseNoiseX = 0.01; // meters + private double baseNoiseY = 0.01; + private double baseNoiseTheta = 0.5; // radians + + private double distanceCoefficientX = 0.055; + private double distanceCoefficientY = 0.055; + private double distanceCoefficientTheta = 1; + + private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle + private double angleCoefficientY = 0.5; + private double angleCoefficientTheta = 0.5; + + private double speedCoefficientX = 0.5; // noise growth per fraction of max speed + private double speedCoefficientY = 0.5; + private double speedCoefficientTheta = 0.5; + + // Maximums for normalization + private double maximumViewingAngle = Math.toRadians(90.0); + private double maximumRobotSpeed = 5; // meters per second + private double maximumAllowedDistance = 15.0; // meters, beyond which readings are dropped + + // PhotonVision and odometry references + private final PhotonCamera photonCamera; + private final PhotonPoseEstimator poseEstimator; // MULTI_TAG_PNP_ON_COPROCESSOR + private PhotonPipelineResult latestVisionResult; + private final BooleanSupplier isRedSide; + private Pose2d lastKnownPose = new Pose2d(0, 0, new Rotation2d()); + private SwerveSubsystem swerveDrive; + private final AprilTagFieldLayout fieldLayout; + + public AnthonyVision(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + this.isRedSide = isRedSide; + this.cameraId = cameraId; + photonCamera = new PhotonCamera(cameraId.toString()); + Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); + + // Initialize field layout + this.fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + + // Initialize both pose estimators + poseEstimator = + new PhotonPoseEstimator( + fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + + latestVisionResult = null; + } + + // Get instance of AnthonyVision + public static AnthonyVision getInstance( + Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + int idx = cameraId.ordinal(); + if (systemList[idx] == null) { + systemList[idx] = new AnthonyVision(cameraId, isRedSide); + } + return systemList[idx]; + } + + @Override + public void periodic() { + // Initialize swerve drive if not already done + if (swerveDrive == null) { + swerveDrive = SwerveSubsystem.getInstance(); + } + + // Check camera connection + boolean cameraConnected = photonCamera.isConnected(); + + // If the current camera isn't connected, there's nothing to do here + if (!cameraConnected) { + return; + } + + // Get all unread results + List results = photonCamera.getAllUnreadResults(); + + // Go through all results (if there are any) and update the latest result with the last + for (var result : results) { + latestVisionResult = result; + } + } + + /** + * Internal method to handle pose estimation with optional confidence checking. + * + * @param useTrigsolve If true, uses trigsolve strategy + * @param forceAdd If true, bypasses confidence checking (used for non-trigsolve or single camera) + * @return PoseEstimateResult containing the pose and confidence data, or null if failed + */ + private void addFilteredPose() { + PhotonPoseEstimator selectedEstimator = poseEstimator; + + if (latestVisionResult == null || !latestVisionResult.hasTargets()) { + return; + } + + // Filter tags to only those on the active side + List validTags = + latestVisionResult.getTargets().stream() + .filter(t -> isTagOnActiveSide(t.getFiducialId())) + .collect(Collectors.toList()); + + // Log all detected tags for debugging + String allTagIds = latestVisionResult.getTargets().stream() + .map(t -> Integer.toString(t.getFiducialId())) + .collect(Collectors.joining(",")); + + if (validTags.isEmpty()) { + return; + } + + // Log all tags that haven't been thrown out + int tagCount = validTags.size(); + String tagIdsCsv = validTags.stream() + .map(t -> Integer.toString(t.getFiducialId())) + .sorted() + .collect(Collectors.joining(",")); + + // Compute effective metrics for solution + // Use camera→target distance from PV (avoids odometry dependence) + double averageDistance = + validTags.stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .average() + .orElse(Double.NaN); + double minDistance = + validTags.stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .min() + .orElse(Double.NaN); + + // nothing to do if rejected based on the minDistance or if no min dist has been found + if (Double.isNaN(minDistance) || minDistance > maximumAllowedDistance) { + return; + } + + // find the current speed + double currentSpeed = Math.hypot( + swerveDrive.getRobotSpeeds().vxMetersPerSecond, + swerveDrive.getRobotSpeeds().vyMetersPerSecond); + + // Get the pose from PhotonVision + Optional maybePose = selectedEstimator.update(latestVisionResult); + if (maybePose.isEmpty()) { + return; + } + + EstimatedRobotPose estimatedPose = maybePose.get(); + Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); + + double nX = computeNoise( + baseNoiseX, distanceCoefficientX, angleCoefficientX, speedCoefficientX, + averageDistance, 0, currentSpeed, tagCount); + double nY = computeNoise( + baseNoiseY, distanceCoefficientY, angleCoefficientY, speedCoefficientY, + averageDistance, 0, currentSpeed, tagCount); + double nTH = computeNoise( + baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, speedCoefficientTheta, + averageDistance, 0, currentSpeed, tagCount); + + Matrix noiseVector = VecBuilder.fill(nX, nY, nTH); + // Process locally (no cross-camera comparison) + processPoseEstimate(measuredPose, averageDistance, + currentSpeed, tagCount, latestVisionResult.getTimestampSeconds(), noiseVector); + } + + /** + * Final processing and addition of pose estimate to odometry. + */ + private void processPoseEstimate(Pose2d measuredPose, double averageDistance, double currentSpeed, int tagCount, double timestamp, Matrix noiseVector) { + lastKnownPose = measuredPose; + + // Choose timestamp: use vision timestamp unless it differs too much from FPGA + double fpgaTimestamp = Timer.getFPGATimestamp(); + double timestampDifference = Math.abs(timestamp - fpgaTimestamp); + double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp-0.03 : timestamp; + + // Build the noise vector and add the vision measurement + + + swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); + } + + private boolean isTagOnActiveSide(int tagId) { + return isRedSide.getAsBoolean() + ? RED_SIDE_TAG_IDS.contains(tagId) + : BLUE_SIDE_TAG_IDS.contains(tagId); + } + + private double computeNoise( + double baseNoise, + double distanceCoefficient, + double angleCoefficient, + double speedCoefficient, + double distance, + double angleRad, + double robotSpeed, + int tagCount) { + + // Tag count factor (diminishing returns; cap at 4) + int effectiveTags = Math.min(tagCount, 4); + double tagFactor = 1.0 / Math.sqrt(effectiveTags); + + // Distance term (keep as d^2) + double distanceFactor = baseNoise + distanceCoefficient * distance * distance; + + // Speed term (quadratic, saturated) + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) + / maximumRobotSpeed; + double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); + + double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; + return computedStdDevs; + } + +} \ No newline at end of file From 077f0bd8b7f89fd83850f80080c3e77671f4f3f5 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 30 Aug 2025 16:27:43 -0700 Subject: [PATCH 24/43] removed angle stuff --- src/main/java/frc/robot/subsystems/AnthonyVision.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 7ac41214..202021c8 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -200,13 +200,13 @@ private void addFilteredPose() { double nX = computeNoise( baseNoiseX, distanceCoefficientX, angleCoefficientX, speedCoefficientX, - averageDistance, 0, currentSpeed, tagCount); + averageDistance, currentSpeed, tagCount); double nY = computeNoise( baseNoiseY, distanceCoefficientY, angleCoefficientY, speedCoefficientY, - averageDistance, 0, currentSpeed, tagCount); + averageDistance, currentSpeed, tagCount); double nTH = computeNoise( baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, speedCoefficientTheta, - averageDistance, 0, currentSpeed, tagCount); + averageDistance, currentSpeed, tagCount); Matrix noiseVector = VecBuilder.fill(nX, nY, nTH); // Process locally (no cross-camera comparison) @@ -243,7 +243,6 @@ private double computeNoise( double angleCoefficient, double speedCoefficient, double distance, - double angleRad, double robotSpeed, int tagCount) { From 88875ce563ac84f91f5461ca76bbfe36c94ecc5c Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 30 Aug 2025 16:28:52 -0700 Subject: [PATCH 25/43] removed lastknownpose --- src/main/java/frc/robot/subsystems/AnthonyVision.java | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 202021c8..ad68b92c 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -12,8 +12,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; @@ -66,7 +64,6 @@ public class AnthonyVision extends SubsystemBase { private double speedCoefficientTheta = 0.5; // Maximums for normalization - private double maximumViewingAngle = Math.toRadians(90.0); private double maximumRobotSpeed = 5; // meters per second private double maximumAllowedDistance = 15.0; // meters, beyond which readings are dropped @@ -75,7 +72,6 @@ public class AnthonyVision extends SubsystemBase { private final PhotonPoseEstimator poseEstimator; // MULTI_TAG_PNP_ON_COPROCESSOR private PhotonPipelineResult latestVisionResult; private final BooleanSupplier isRedSide; - private Pose2d lastKnownPose = new Pose2d(0, 0, new Rotation2d()); private SwerveSubsystem swerveDrive; private final AprilTagFieldLayout fieldLayout; @@ -218,8 +214,6 @@ private void addFilteredPose() { * Final processing and addition of pose estimate to odometry. */ private void processPoseEstimate(Pose2d measuredPose, double averageDistance, double currentSpeed, int tagCount, double timestamp, Matrix noiseVector) { - lastKnownPose = measuredPose; - // Choose timestamp: use vision timestamp unless it differs too much from FPGA double fpgaTimestamp = Timer.getFPGATimestamp(); double timestampDifference = Math.abs(timestamp - fpgaTimestamp); From 60c3363a974e709e6eb9ef9cd70fce85654f795a Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 30 Aug 2025 17:21:16 -0700 Subject: [PATCH 26/43] fixed stuff --- src/main/java/frc/robot/Robot.java | 13 +++++++------ .../frc/robot/subsystems/AnthonyVision.java | 18 ++++++++++++++++-- 2 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 66dca234..0e333337 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.commands.ElevatorCommands.ZeroElevatorHardStop; +import frc.robot.subsystems.AnthonyVision; import frc.robot.subsystems.CoralPosition; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.SwerveSubsystem; @@ -34,8 +35,8 @@ public class Robot extends TimedRobot { // private VisionSystem visionLeft = VisionSystem.getInstance(Constants.Vision.Cameras.LEFT_CAM); private SwerveSubsystem driveTrain = SwerveSubsystem.getInstance(); private final RobotContainer m_robotContainer; - private VisionSystem visionRight; - private VisionSystem visionLeft; + private AnthonyVision visionRight; + private AnthonyVision visionLeft; // standard deviation for x (meters), y (meters) and rotation (radians) camera data @@ -50,9 +51,9 @@ public Robot() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); visionRight = - VisionSystem.getInstance(Constants.Vision.Cameras.RIGHT_CAM, m_robotContainer.getRedSide()); + AnthonyVision.getInstance(Constants.Vision.Cameras.RIGHT_CAM, m_robotContainer.getRedSide()); visionLeft = - VisionSystem.getInstance(Constants.Vision.Cameras.LEFT_CAM, m_robotContainer.getRedSide()); + AnthonyVision.getInstance(Constants.Vision.Cameras.LEFT_CAM, m_robotContainer.getRedSide()); absoluteInit(); } @@ -71,8 +72,8 @@ public void robotPeriodic() { CommandScheduler.getInstance().run(); m_robotContainer.doTelemetry(); - visionRight.addFilteredPose(false); - visionLeft.addFilteredPose(false); + visionRight.addFilteredPose(); + visionLeft.addFilteredPose(); DogLog.log("KalmanDebug/drivetrainPose", driveTrain.getPose()); diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index ad68b92c..adac65e0 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -133,7 +133,7 @@ public void periodic() { * @param forceAdd If true, bypasses confidence checking (used for non-trigsolve or single camera) * @return PoseEstimateResult containing the pose and confidence data, or null if failed */ - private void addFilteredPose() { + public void addFilteredPose() { PhotonPoseEstimator selectedEstimator = poseEstimator; if (latestVisionResult == null || !latestVisionResult.hasTargets()) { @@ -145,7 +145,21 @@ private void addFilteredPose() { latestVisionResult.getTargets().stream() .filter(t -> isTagOnActiveSide(t.getFiducialId())) .collect(Collectors.toList()); - + + if (cameraId.equals(Constants.Vision.Cameras.RIGHT_CAM)) { + for (PhotonTrackedTarget tag : validTags) { + DogLog.log("Vision/RightCAM/Area", tag.getArea()); + DogLog.log("Vision/RightCAM/Yaw", tag.getYaw()); + } + } + + if (cameraId.equals(Constants.Vision.Cameras.LEFT_CAM)) { + for (PhotonTrackedTarget tag : validTags) { + DogLog.log("Vision/LeftCAM/Area", tag.getArea()); + DogLog.log("Vision/LefttCAM/Yaw", tag.getYaw()); + } + } + // Log all detected tags for debugging String allTagIds = latestVisionResult.getTargets().stream() .map(t -> Integer.toString(t.getFiducialId())) From 5713467873f5220e0308be48c3904400db1430f9 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 30 Aug 2025 17:21:47 -0700 Subject: [PATCH 27/43] removed space --- src/main/java/frc/robot/subsystems/AnthonyVision.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index adac65e0..195e6b03 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -269,5 +269,4 @@ private double computeNoise( double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; return computedStdDevs; } - } \ No newline at end of file From 86d8aa76a8c978e518101b2f2492d83cd3d3d19a Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 30 Aug 2025 18:26:37 -0700 Subject: [PATCH 28/43] visioncommit --- .../frc/robot/subsystems/AnthonyVision.java | 63 ++++++++++--------- 1 file changed, 34 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 195e6b03..32b2eb40 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -51,8 +51,8 @@ public class AnthonyVision extends SubsystemBase { private double baseNoiseY = 0.01; private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.055; - private double distanceCoefficientY = 0.055; + private double distanceCoefficientX = 0.06; + private double distanceCoefficientY = 0.06; private double distanceCoefficientTheta = 1; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle @@ -135,59 +135,53 @@ public void periodic() { */ public void addFilteredPose() { PhotonPoseEstimator selectedEstimator = poseEstimator; - + String camTitle = cameraId.getLoggingName(); if (latestVisionResult == null || !latestVisionResult.hasTargets()) { return; } + double averageDistance = + latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .average() + .orElse(Double.NaN); + + double minDistance = + latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .min() + .orElse(Double.NaN); + // Filter tags to only those on the active side List validTags = latestVisionResult.getTargets().stream() .filter(t -> isTagOnActiveSide(t.getFiducialId())) + .filter(t -> isNotChopped(t.getYaw())) .collect(Collectors.toList()); - - if (cameraId.equals(Constants.Vision.Cameras.RIGHT_CAM)) { - for (PhotonTrackedTarget tag : validTags) { - DogLog.log("Vision/RightCAM/Area", tag.getArea()); - DogLog.log("Vision/RightCAM/Yaw", tag.getYaw()); - } - } + + DogLog.log("Vision/"+camTitle+"/numValidTags", validTags.size()); - if (cameraId.equals(Constants.Vision.Cameras.LEFT_CAM)) { for (PhotonTrackedTarget tag : validTags) { - DogLog.log("Vision/LeftCAM/Area", tag.getArea()); - DogLog.log("Vision/LefttCAM/Yaw", tag.getYaw()); + DogLog.log("Vision/"+camTitle+"/Area", tag.getArea()); + DogLog.log("Vision/"+camTitle+"/Yaw", tag.getYaw()); } - } // Log all detected tags for debugging String allTagIds = latestVisionResult.getTargets().stream() .map(t -> Integer.toString(t.getFiducialId())) .collect(Collectors.joining(",")); - + + DogLog.log("Vision/"+camTitle+"/allTagIds", allTagIds); + if (validTags.isEmpty()) { return; } // Log all tags that haven't been thrown out int tagCount = validTags.size(); - String tagIdsCsv = validTags.stream() - .map(t -> Integer.toString(t.getFiducialId())) - .sorted() - .collect(Collectors.joining(",")); // Compute effective metrics for solution // Use camera→target distance from PV (avoids odometry dependence) - double averageDistance = - validTags.stream() - .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) - .average() - .orElse(Double.NaN); - double minDistance = - validTags.stream() - .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) - .min() - .orElse(Double.NaN); // nothing to do if rejected based on the minDistance or if no min dist has been found if (Double.isNaN(minDistance) || minDistance > maximumAllowedDistance) { @@ -218,6 +212,13 @@ public void addFilteredPose() { baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, speedCoefficientTheta, averageDistance, currentSpeed, tagCount); + DogLog.log("Vision/"+camTitle+"/speed", currentSpeed); + DogLog.log("Vision/"+camTitle+"/nX", nX); + DogLog.log("Vision/"+camTitle+"/nY", nY); + DogLog.log("Vision/"+camTitle+"/nTH", nTH); + DogLog.log("Vision/"+camTitle+"/Pose", measuredPose); + DogLog.log("Vision/"+camTitle+"/averageDistance", averageDistance); + Matrix noiseVector = VecBuilder.fill(nX, nY, nTH); // Process locally (no cross-camera comparison) processPoseEstimate(measuredPose, averageDistance, @@ -245,6 +246,10 @@ private boolean isTagOnActiveSide(int tagId) { : BLUE_SIDE_TAG_IDS.contains(tagId); } + private boolean isNotChopped(double yaw) { + return (Math.abs(yaw) < 60d); + } + private double computeNoise( double baseNoise, double distanceCoefficient, From d956d73b620de52e331211035acf98319d0d23eb Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 11:43:32 -0700 Subject: [PATCH 29/43] vision tweak during drive practice --- src/main/java/frc/robot/subsystems/AnthonyVision.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 32b2eb40..22b8dd3a 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -53,7 +53,7 @@ public class AnthonyVision extends SubsystemBase { private double distanceCoefficientX = 0.06; private double distanceCoefficientY = 0.06; - private double distanceCoefficientTheta = 1; + private double distanceCoefficientTheta = 0.9; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle private double angleCoefficientY = 0.5; From a3de590a94b8c54fa61e3d52c517069088c6677d Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 12:48:54 -0700 Subject: [PATCH 30/43] updated vendordeps --- vendordeps/DogLog.json | 4 +- vendordeps/Phoenix6-frc2025-latest.json | 116 ++++++++++++++++++------ 2 files changed, 90 insertions(+), 30 deletions(-) diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json index b410eb1d..2ee13f69 100644 --- a/vendordeps/DogLog.json +++ b/vendordeps/DogLog.json @@ -3,7 +3,7 @@ { "groupId": "com.github.jonahsnider", "artifactId": "doglog", - "version": "2025.3.0" + "version": "2025.8.1" } ], "fileName": "DogLog.json", @@ -15,6 +15,6 @@ "https://jitpack.io" ], "cppDependencies": [], - "version": "2025.3.0", + "version": "2025.8.1", "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" } \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index acc78dba..6f40c840 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.2", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,35 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +442,38 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file From 2f25304cbe0f1ec33d650870db4553225e624f61 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 12:49:22 -0700 Subject: [PATCH 31/43] spotless --- src/main/java/frc/robot/Robot.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 1 - .../frc/robot/subsystems/AnthonyVision.java | 142 +++++---- .../frc/robot/subsystems/VisionSystem.java | 300 +++++++++++------- 4 files changed, 280 insertions(+), 167 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0e333337..a9cd8a8e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -14,7 +14,6 @@ import frc.robot.subsystems.CoralPosition; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.SwerveSubsystem; -import frc.robot.subsystems.VisionSystem; import frc.robot.util.LoggedTalonFX; /** @@ -51,7 +50,8 @@ public Robot() { // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); visionRight = - AnthonyVision.getInstance(Constants.Vision.Cameras.RIGHT_CAM, m_robotContainer.getRedSide()); + AnthonyVision.getInstance( + Constants.Vision.Cameras.RIGHT_CAM, m_robotContainer.getRedSide()); visionLeft = AnthonyVision.getInstance(Constants.Vision.Cameras.LEFT_CAM, m_robotContainer.getRedSide()); absoluteInit(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7823a556..404e2792 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 22b8dd3a..0f6c7eae 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -10,7 +10,6 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -51,7 +50,7 @@ public class AnthonyVision extends SubsystemBase { private double baseNoiseY = 0.01; private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.06; + private double distanceCoefficientX = 0.06; private double distanceCoefficientY = 0.06; private double distanceCoefficientTheta = 0.9; @@ -80,15 +79,15 @@ public AnthonyVision(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSid this.cameraId = cameraId; photonCamera = new PhotonCamera(cameraId.toString()); Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); - + // Initialize field layout this.fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - - // Initialize both pose estimators - poseEstimator = - new PhotonPoseEstimator( - fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); - + + // Initialize both pose estimators + poseEstimator = + new PhotonPoseEstimator( + fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + latestVisionResult = null; } @@ -106,7 +105,7 @@ public static AnthonyVision getInstance( public void periodic() { // Initialize swerve drive if not already done if (swerveDrive == null) { - swerveDrive = SwerveSubsystem.getInstance(); + swerveDrive = SwerveSubsystem.getInstance(); } // Check camera connection @@ -128,14 +127,14 @@ public void periodic() { /** * Internal method to handle pose estimation with optional confidence checking. - * + * * @param useTrigsolve If true, uses trigsolve strategy * @param forceAdd If true, bypasses confidence checking (used for non-trigsolve or single camera) * @return PoseEstimateResult containing the pose and confidence data, or null if failed */ public void addFilteredPose() { PhotonPoseEstimator selectedEstimator = poseEstimator; - String camTitle = cameraId.getLoggingName(); + String camTitle = cameraId.getLoggingName(); if (latestVisionResult == null || !latestVisionResult.hasTargets()) { return; } @@ -158,25 +157,26 @@ public void addFilteredPose() { .filter(t -> isTagOnActiveSide(t.getFiducialId())) .filter(t -> isNotChopped(t.getYaw())) .collect(Collectors.toList()); - - DogLog.log("Vision/"+camTitle+"/numValidTags", validTags.size()); - for (PhotonTrackedTarget tag : validTags) { - DogLog.log("Vision/"+camTitle+"/Area", tag.getArea()); - DogLog.log("Vision/"+camTitle+"/Yaw", tag.getYaw()); - } + DogLog.log("Vision/" + camTitle + "/numValidTags", validTags.size()); + + for (PhotonTrackedTarget tag : validTags) { + DogLog.log("Vision/" + camTitle + "/Area", tag.getArea()); + DogLog.log("Vision/" + camTitle + "/Yaw", tag.getYaw()); + } // Log all detected tags for debugging - String allTagIds = latestVisionResult.getTargets().stream() - .map(t -> Integer.toString(t.getFiducialId())) - .collect(Collectors.joining(",")); - - DogLog.log("Vision/"+camTitle+"/allTagIds", allTagIds); + String allTagIds = + latestVisionResult.getTargets().stream() + .map(t -> Integer.toString(t.getFiducialId())) + .collect(Collectors.joining(",")); + + DogLog.log("Vision/" + camTitle + "/allTagIds", allTagIds); if (validTags.isEmpty()) { return; } - + // Log all tags that haven't been thrown out int tagCount = validTags.size(); @@ -189,54 +189,81 @@ public void addFilteredPose() { } // find the current speed - double currentSpeed = Math.hypot( - swerveDrive.getRobotSpeeds().vxMetersPerSecond, - swerveDrive.getRobotSpeeds().vyMetersPerSecond); - + double currentSpeed = + Math.hypot( + swerveDrive.getRobotSpeeds().vxMetersPerSecond, + swerveDrive.getRobotSpeeds().vyMetersPerSecond); + // Get the pose from PhotonVision Optional maybePose = selectedEstimator.update(latestVisionResult); if (maybePose.isEmpty()) { return; } - + EstimatedRobotPose estimatedPose = maybePose.get(); Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); - - double nX = computeNoise( - baseNoiseX, distanceCoefficientX, angleCoefficientX, speedCoefficientX, - averageDistance, currentSpeed, tagCount); - double nY = computeNoise( - baseNoiseY, distanceCoefficientY, angleCoefficientY, speedCoefficientY, - averageDistance, currentSpeed, tagCount); - double nTH = computeNoise( - baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, speedCoefficientTheta, - averageDistance, currentSpeed, tagCount); - - DogLog.log("Vision/"+camTitle+"/speed", currentSpeed); - DogLog.log("Vision/"+camTitle+"/nX", nX); - DogLog.log("Vision/"+camTitle+"/nY", nY); - DogLog.log("Vision/"+camTitle+"/nTH", nTH); - DogLog.log("Vision/"+camTitle+"/Pose", measuredPose); - DogLog.log("Vision/"+camTitle+"/averageDistance", averageDistance); + + double nX = + computeNoise( + baseNoiseX, + distanceCoefficientX, + angleCoefficientX, + speedCoefficientX, + averageDistance, + currentSpeed, + tagCount); + double nY = + computeNoise( + baseNoiseY, + distanceCoefficientY, + angleCoefficientY, + speedCoefficientY, + averageDistance, + currentSpeed, + tagCount); + double nTH = + computeNoise( + baseNoiseTheta, + distanceCoefficientTheta, + angleCoefficientTheta, + speedCoefficientTheta, + averageDistance, + currentSpeed, + tagCount); + + DogLog.log("Vision/" + camTitle + "/speed", currentSpeed); + DogLog.log("Vision/" + camTitle + "/nX", nX); + DogLog.log("Vision/" + camTitle + "/nY", nY); + DogLog.log("Vision/" + camTitle + "/nTH", nTH); + DogLog.log("Vision/" + camTitle + "/Pose", measuredPose); + DogLog.log("Vision/" + camTitle + "/averageDistance", averageDistance); Matrix noiseVector = VecBuilder.fill(nX, nY, nTH); // Process locally (no cross-camera comparison) - processPoseEstimate(measuredPose, averageDistance, - currentSpeed, tagCount, latestVisionResult.getTimestampSeconds(), noiseVector); + processPoseEstimate( + measuredPose, + averageDistance, + currentSpeed, + tagCount, + latestVisionResult.getTimestampSeconds(), + noiseVector); } - /** - * Final processing and addition of pose estimate to odometry. - */ - private void processPoseEstimate(Pose2d measuredPose, double averageDistance, double currentSpeed, int tagCount, double timestamp, Matrix noiseVector) { + /** Final processing and addition of pose estimate to odometry. */ + private void processPoseEstimate( + Pose2d measuredPose, + double averageDistance, + double currentSpeed, + int tagCount, + double timestamp, + Matrix noiseVector) { // Choose timestamp: use vision timestamp unless it differs too much from FPGA double fpgaTimestamp = Timer.getFPGATimestamp(); double timestampDifference = Math.abs(timestamp - fpgaTimestamp); - double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp-0.03 : timestamp; + double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp - 0.03 : timestamp; // Build the noise vector and add the vision measurement - - + swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); } @@ -267,11 +294,10 @@ private double computeNoise( double distanceFactor = baseNoise + distanceCoefficient * distance * distance; // Speed term (quadratic, saturated) - double vNorm = Math.min(robotSpeed, maximumRobotSpeed) - / maximumRobotSpeed; + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; return computedStdDevs; } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java index 80e4d297..2d937f3d 100644 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ b/src/main/java/frc/robot/subsystems/VisionSystem.java @@ -11,9 +11,9 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.Timer; @@ -83,7 +83,7 @@ public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide this.cameraId = cameraId; photonCamera = new PhotonCamera(cameraId.toString()); Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); - + // Initialize field layout with error handling AprilTagFieldLayout tempLayout = null; try { @@ -93,23 +93,22 @@ public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide e.printStackTrace(); } this.fieldLayout = tempLayout; - + // Initialize both pose estimators only if field layout loaded successfully if (fieldLayout != null) { poseEstimator = new PhotonPoseEstimator( fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); - + trigsolvePoseEstimator = - new PhotonPoseEstimator( - fieldLayout, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, cameraToRobot); + new PhotonPoseEstimator(fieldLayout, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, cameraToRobot); } else { poseEstimator = null; trigsolvePoseEstimator = null; } - + latestVisionResult = null; - + // Minimal debug logging (guarded by ENABLE_LOGS) v("Init", true); v("FieldLayoutLoaded", fieldLayout != null); @@ -174,8 +173,9 @@ public void periodic() { } /** - * Attempts to fuse a vision measurement into the swerve pose estimator using MULTI_TAG_PNP strategy, - * dropping readings that fail validity checks, and computing noise dynamically via computeMeasurementNoise(). + * Attempts to fuse a vision measurement into the swerve pose estimator using MULTI_TAG_PNP + * strategy, dropping readings that fail validity checks, and computing noise dynamically via + * computeMeasurementNoise(). */ public void addFilteredPose() { addFilteredPose(false); @@ -184,9 +184,9 @@ public void addFilteredPose() { /** * Attempts to fuse a vision measurement into the swerve pose estimator, dropping readings that * fail validity checks, and computing noise dynamically via computeMeasurementNoise(). - * - * @param useTrigsolve If true, uses the trigsolve pose estimator (LOWEST_AMBIGUITY strategy). - * If false, uses the multi-tag PnP pose estimator (MULTI_TAG_PNP_ON_COPROCESSOR strategy). + * + * @param useTrigsolve If true, uses the trigsolve pose estimator (LOWEST_AMBIGUITY strategy). If + * false, uses the multi-tag PnP pose estimator (MULTI_TAG_PNP_ON_COPROCESSOR strategy). */ public void addFilteredPose(boolean useTrigsolve) { addFilteredPoseInternal(useTrigsolve, false); @@ -194,31 +194,31 @@ public void addFilteredPose(boolean useTrigsolve) { /** * Internal method to handle pose estimation with optional confidence checking. - * + * * @param useTrigsolve If true, uses trigsolve strategy * @param forceAdd If true, bypasses confidence checking (used for non-trigsolve or single camera) * @return PoseEstimateResult containing the pose and confidence data, or null if failed */ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean forceAdd) { final String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; - + // Check prerequisites if (fieldLayout == null) { v(strategyPrefix + "/FieldLayoutMissing", true); return null; } - + if (swerveDrive == null) { v(strategyPrefix + "/SwerveDriveNull", true); return null; } - + PhotonPoseEstimator selectedEstimator = useTrigsolve ? trigsolvePoseEstimator : poseEstimator; if (selectedEstimator == null) { v(strategyPrefix + "/PoseEstimatorNull", true); return null; } - + if (latestVisionResult == null || !latestVisionResult.hasTargets()) { if (!forceAdd) { v(strategyPrefix + "/HasTargets", false); @@ -234,27 +234,29 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean latestVisionResult.getTargets().stream() .filter(t -> isTagOnActiveSide(t.getFiducialId())) .collect(Collectors.toList()); - + // Log all detected tags for debugging - String allTagIds = latestVisionResult.getTargets().stream() - .map(t -> Integer.toString(t.getFiducialId())) - .collect(Collectors.joining(",")); + String allTagIds = + latestVisionResult.getTargets().stream() + .map(t -> Integer.toString(t.getFiducialId())) + .collect(Collectors.joining(",")); v(strategyPrefix + "/AllDetectedTags", allTagIds); v(strategyPrefix + "/IsRedSide", isRedSide.getAsBoolean()); - + if (validTags.isEmpty()) { if (!forceAdd) { v(strategyPrefix + "/TagFilter", false); } return null; } - + int tagCount = validTags.size(); v(strategyPrefix + "/Tags/Count", tagCount); - String tagIdsCsv = validTags.stream() - .map(t -> Integer.toString(t.getFiducialId())) - .sorted() - .collect(Collectors.joining(",")); + String tagIdsCsv = + validTags.stream() + .map(t -> Integer.toString(t.getFiducialId())) + .sorted() + .collect(Collectors.joining(",")); v(strategyPrefix + "/Tags/IDs", tagIdsCsv); // Compute effective metrics for solution @@ -292,25 +294,26 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean v(strategyPrefix + "/Skew/RmsDeg", Math.toDegrees(rmsSkewRad)); v(strategyPrefix + "/Skew/MaxDeg", Math.toDegrees(maxSkewRad)); -// No skew gating: skew only affects noise scaling below. + // No skew gating: skew only affects noise scaling below. -// Use strategy-appropriate skew for noise scaling -double averageAngle = useTrigsolve ? minSkewRad : rmsSkewRad; + // Use strategy-appropriate skew for noise scaling + double averageAngle = useTrigsolve ? minSkewRad : rmsSkewRad; double currentSpeed = 0.0; try { - currentSpeed = Math.hypot( - swerveDrive.getRobotSpeeds().vxMetersPerSecond, - swerveDrive.getRobotSpeeds().vyMetersPerSecond); + currentSpeed = + Math.hypot( + swerveDrive.getRobotSpeeds().vxMetersPerSecond, + swerveDrive.getRobotSpeeds().vyMetersPerSecond); } catch (Exception e) { v(strategyPrefix + "/SpeedCalculationError", true); } - + v(strategyPrefix + "/Inputs/SkewUsedDeg", Math.toDegrees(averageAngle)); v(strategyPrefix + "/Inputs/SpeedMps", currentSpeed); // Choose the appropriate pose estimator and reference pose Pose2d referencePos = useTrigsolve ? lastKnownTrigsolvePose : lastKnownPose; - + // Update reference pose with current odometry if we have it try { Pose2d currentOdometry = swerveDrive.getPose(); @@ -324,7 +327,7 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean v(strategyPrefix + "/ReferencePoseError", true); selectedEstimator.setReferencePose(referencePos); } - + // Get the pose from PhotonVision Optional maybePose = selectedEstimator.update(latestVisionResult); if (maybePose.isEmpty()) { @@ -334,24 +337,45 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean } return null; } - + EstimatedRobotPose estimatedPose = maybePose.get(); Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); - + // Calculate confidence metric for trigsolve based on measurement quality double confidence = 1.0; // Default confidence for multi-tag if (useTrigsolve) { // For trigsolve, calculate confidence based on the measurement noise multipliers // Lower total noise = higher confidence - NoiseComponents nxC = computeNoiseComponents( - baseNoiseX, distanceCoefficientX, angleCoefficientX, speedCoefficientX, - averageDistance, averageAngle, currentSpeed, tagCount); - NoiseComponents nyC = computeNoiseComponents( - baseNoiseY, distanceCoefficientY, angleCoefficientY, speedCoefficientY, - averageDistance, averageAngle, currentSpeed, tagCount); - NoiseComponents nthC = computeNoiseComponents( - baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, speedCoefficientTheta, - averageDistance, averageAngle, currentSpeed, tagCount); + NoiseComponents nxC = + computeNoiseComponents( + baseNoiseX, + distanceCoefficientX, + angleCoefficientX, + speedCoefficientX, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + NoiseComponents nyC = + computeNoiseComponents( + baseNoiseY, + distanceCoefficientY, + angleCoefficientY, + speedCoefficientY, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + NoiseComponents nthC = + computeNoiseComponents( + baseNoiseTheta, + distanceCoefficientTheta, + angleCoefficientTheta, + speedCoefficientTheta, + averageDistance, + averageAngle, + currentSpeed, + tagCount); double noiseX = nxC.total; double noiseY = nyC.total; @@ -380,36 +404,73 @@ private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean v(strategyPrefix + "/Noise/Theta/AngleTerm", nthC.angleTerm); v(strategyPrefix + "/Noise/Theta/SpeedTerm", nthC.speedTerm); } - + // If this is just for confidence comparison, return the result without adding to odometry if (forceAdd) { - return new PoseEstimateResult(measuredPose, confidence, averageDistance, averageAngle, - currentSpeed, tagCount, latestVisionResult.getTimestampSeconds()); + return new PoseEstimateResult( + measuredPose, + confidence, + averageDistance, + averageAngle, + currentSpeed, + tagCount, + latestVisionResult.getTimestampSeconds()); } // Process locally (no cross-camera comparison) - return processPoseEstimate(measuredPose, useTrigsolve, averageDistance, averageAngle, - currentSpeed, tagCount, latestVisionResult.getTimestampSeconds()); + return processPoseEstimate( + measuredPose, + useTrigsolve, + averageDistance, + averageAngle, + currentSpeed, + tagCount, + latestVisionResult.getTimestampSeconds()); } // (processTrigsolveWithConfidenceCheck removed) - /** - * Final processing and addition of pose estimate to odometry. - */ - private PoseEstimateResult processPoseEstimate(Pose2d measuredPose, boolean useTrigsolve, - double averageDistance, double averageAngle, double currentSpeed, int tagCount, double timestamp) { - + /** Final processing and addition of pose estimate to odometry. */ + private PoseEstimateResult processPoseEstimate( + Pose2d measuredPose, + boolean useTrigsolve, + double averageDistance, + double averageAngle, + double currentSpeed, + int tagCount, + double timestamp) { + // Compute measurement noise for each axis and log components - NoiseComponents nxC = computeNoiseComponents( - baseNoiseX, distanceCoefficientX, angleCoefficientX, speedCoefficientX, - averageDistance, averageAngle, currentSpeed, tagCount); - NoiseComponents nyC = computeNoiseComponents( - baseNoiseY, distanceCoefficientY, angleCoefficientY, speedCoefficientY, - averageDistance, averageAngle, currentSpeed, tagCount); - NoiseComponents nthC = computeNoiseComponents( - baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, speedCoefficientTheta, - averageDistance, averageAngle, currentSpeed, tagCount); + NoiseComponents nxC = + computeNoiseComponents( + baseNoiseX, + distanceCoefficientX, + angleCoefficientX, + speedCoefficientX, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + NoiseComponents nyC = + computeNoiseComponents( + baseNoiseY, + distanceCoefficientY, + angleCoefficientY, + speedCoefficientY, + averageDistance, + averageAngle, + currentSpeed, + tagCount); + NoiseComponents nthC = + computeNoiseComponents( + baseNoiseTheta, + distanceCoefficientTheta, + angleCoefficientTheta, + speedCoefficientTheta, + averageDistance, + averageAngle, + currentSpeed, + tagCount); double noiseX = nxC.total; double noiseY = nyC.total; @@ -449,7 +510,7 @@ private PoseEstimateResult processPoseEstimate(Pose2d measuredPose, boolean useT // Build the noise vector and add the vision measurement Matrix noiseVector = VecBuilder.fill(noiseX, noiseY, noiseTheta); - + try { swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); v(strategyPrefix + "/MeasurementUsed", true); @@ -460,8 +521,8 @@ private PoseEstimateResult processPoseEstimate(Pose2d measuredPose, boolean useT vts(strategyPrefix + "/Frame/End"); - return new PoseEstimateResult(measuredPose, 1.0, averageDistance, averageAngle, - currentSpeed, tagCount, timestamp); + return new PoseEstimateResult( + measuredPose, 1.0, averageDistance, averageAngle, currentSpeed, tagCount, timestamp); } private boolean isTagOnActiveSide(int tagId) { @@ -474,19 +535,42 @@ private boolean isTagOnActiveSide(int tagId) { // ─── Vision logging helpers ────────────────────────────────────────────────── private static final boolean ENABLE_LOGS = true; // flip true when tuning - private String vKey(String subkey) { return "Vision/" + cameraId.toString() + "/" + subkey; } - private void vts(String subkey) { if (!ENABLE_LOGS) return; DogLog.timestamp(vKey(subkey)); } - private void v(String subkey, double val) { if (!ENABLE_LOGS) return; DogLog.log(vKey(subkey), val); } - private void v(String subkey, boolean val) { if (!ENABLE_LOGS) return; DogLog.log(vKey(subkey), val); } - private void v(String subkey, String val) { if (!ENABLE_LOGS) return; DogLog.log(vKey(subkey), val); } - private void v(String subkey, Pose2d val) { if (!ENABLE_LOGS) return; DogLog.log(vKey(subkey), val); } + + private String vKey(String subkey) { + return "Vision/" + cameraId.toString() + "/" + subkey; + } + + private void vts(String subkey) { + if (!ENABLE_LOGS) return; + DogLog.timestamp(vKey(subkey)); + } + + private void v(String subkey, double val) { + if (!ENABLE_LOGS) return; + DogLog.log(vKey(subkey), val); + } + + private void v(String subkey, boolean val) { + if (!ENABLE_LOGS) return; + DogLog.log(vKey(subkey), val); + } + + private void v(String subkey, String val) { + if (!ENABLE_LOGS) return; + DogLog.log(vKey(subkey), val); + } + + private void v(String subkey, Pose2d val) { + if (!ENABLE_LOGS) return; + DogLog.log(vKey(subkey), val); + } private static class NoiseComponents { - double tagScale; // 1/sqrt(tags) + double tagScale; // 1/sqrt(tags) double distanceTerm; // base + k*d^2 - double angleTerm; // cosine-based (1 - cos θ) normalized - double speedTerm; // 1 + k*(v/vmax)^2 - double total; // calibrationFactor * tagScale * distanceTerm * angleTerm * speedTerm + double angleTerm; // cosine-based (1 - cos θ) normalized + double speedTerm; // 1 + k*(v/vmax)^2 + double total; // calibrationFactor * tagScale * distanceTerm * angleTerm * speedTerm } private NoiseComponents computeNoiseComponents( @@ -517,8 +601,8 @@ private NoiseComponents computeNoiseComponents( c.angleTerm = 1.0 + angleCoefficient * normalizedAngle; // Speed term (quadratic, saturated) - double vNorm = Math.max(0.0, Math.min(robotSpeed, maximumRobotSpeed)) - / Math.max(maximumRobotSpeed, 1e-6); + double vNorm = + Math.max(0.0, Math.min(robotSpeed, maximumRobotSpeed)) / Math.max(maximumRobotSpeed, 1e-6); c.speedTerm = 1.0 + speedCoefficient * (vNorm * vNorm); c.total = calibrationFactor * c.tagScale * c.distanceTerm * c.angleTerm * c.speedTerm; @@ -534,21 +618,20 @@ private double computeMeasurementNoise( double angleRad, double robotSpeed, int tagCount) { - NoiseComponents c = computeNoiseComponents( - baseNoise, - distanceCoefficient, - angleCoefficient, - speedCoefficient, - distance, - angleRad, - robotSpeed, - tagCount); + NoiseComponents c = + computeNoiseComponents( + baseNoise, + distanceCoefficient, + angleCoefficient, + speedCoefficient, + distance, + angleRad, + robotSpeed, + tagCount); return c.total; } - - /** - * Helper class to store pose estimation results with confidence metrics. - */ + + /** Helper class to store pose estimation results with confidence metrics. */ private static class PoseEstimateResult { public final Pose2d pose; public final double confidence; @@ -558,8 +641,14 @@ private static class PoseEstimateResult { public final int tagCount; public final double timestamp; - public PoseEstimateResult(Pose2d pose, double confidence, double averageDistance, - double averageAngle, double currentSpeed, int tagCount, double timestamp) { + public PoseEstimateResult( + Pose2d pose, + double confidence, + double averageDistance, + double averageAngle, + double currentSpeed, + int tagCount, + double timestamp) { this.pose = pose; this.confidence = confidence; this.averageDistance = averageDistance; @@ -569,11 +658,11 @@ public PoseEstimateResult(Pose2d pose, double confidence, double averageDistance this.timestamp = timestamp; } } - + /** - * Computes the true 3D off-axis viewing angle (skew) for a tag. - * Returns angle in radians: 0 = head-on, increases with obliqueness. - * Geometric skew: ACUTE angle between camera +X and tag face normal (−Z_tag). + * Computes the true 3D off-axis viewing angle (skew) for a tag. Returns angle in radians: 0 = + * head-on, increases with obliqueness. Geometric skew: ACUTE angle between camera +X and tag face + * normal (−Z_tag). */ private double computeTargetSkewRad(PhotonTrackedTarget t) { // Geometric skew: ACUTE angle between camera +X and tag face normal (−Z_tag). @@ -596,9 +685,8 @@ private double computeTargetSkewRad(PhotonTrackedTarget t) { } /** - * Weighted RMS of per-tag skew angles (radians) using geometric skew. - * Weights favor closer and front-on tags: - * w_i = cos^2(skew_i) / (d_i^2 + eps) + * Weighted RMS of per-tag skew angles (radians) using geometric skew. Weights favor closer and + * front-on tags: w_i = cos^2(skew_i) / (d_i^2 + eps) */ private double computeWeightedRmsSkewRad(List tags) { double num = 0.0, den = 0.0; @@ -613,4 +701,4 @@ private double computeWeightedRmsSkewRad(List tags) { if (den <= 1e-9) return 0.0; return Math.sqrt(num / den); } -} \ No newline at end of file +} From d2277d8efd46ac19ab667e3317d5720c2a101a18 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 13:13:21 -0700 Subject: [PATCH 32/43] Corrected Swerve logging --- src/main/java/frc/robot/Robot.java | 1 - .../frc/robot/subsystems/SwerveSubsystem.java | 31 ++----------------- 2 files changed, 3 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a9cd8a8e..893d398b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -75,7 +75,6 @@ public void robotPeriodic() { visionRight.addFilteredPose(); visionLeft.addFilteredPose(); - DogLog.log("KalmanDebug/drivetrainPose", driveTrain.getPose()); DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel()); DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide()); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 72b21422..7a69d938 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -455,25 +455,6 @@ public void addVisionMeasurement( @Override public void periodic() { - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - currentState = getState(); if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { @@ -488,16 +469,10 @@ public void periodic() { }); } - DogLog.log("subsystems/swerve/fl_speed", currentState.ModuleStates[0].speedMetersPerSecond); - DogLog.log("subsystems/swerve/fl_angle", currentState.ModuleStates[0].angle.getDegrees()); - DogLog.log("subsystems/swerve/fr_speed", currentState.ModuleStates[1].speedMetersPerSecond); - DogLog.log("subsystems/swerve/fr_angle", currentState.ModuleStates[1].angle.getDegrees()); - DogLog.log("subsystems/swerve/bl_speed", currentState.ModuleStates[2].speedMetersPerSecond); - DogLog.log("subsystems/swerve/bl_angle", currentState.ModuleStates[2].angle.getDegrees()); - DogLog.log("subsystems/swerve/br_speed", currentState.ModuleStates[3].speedMetersPerSecond); - DogLog.log("subsystems/swerve/br_angle", currentState.ModuleStates[3].angle.getDegrees()); + DogLog.log("PoseEstimation/DrivetrainPose", currentState.Pose); + DogLog.log( - "subsystems/swerve/command", + "Subsystems/SwerveSubsystem/CurrentCommand", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } } From ca14d82cfc98bea85f1a5c2b3e9f9248cc6a7983 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 13:18:25 -0700 Subject: [PATCH 33/43] reworked edward logging --- .../frc/robot/subsystems/SwerveSubsystem.java | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 7a69d938..c91243a1 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -344,39 +344,39 @@ public ChassisSpeeds calculateRequiredEdwardChassisSpeeds( Rotation2d travelAngle = travelAngleTo(targetPose); DogLog.log( - "EdwardCalculation/qProfiledPID/CurrentPositionMeasurement", + "Commands/JamesHarden/Edward/qPositionMeasurement", completePathDistance - distanceToTarget); DogLog.log( - "EdwardCalculation/qProfiledPID/PositionSetpoint", + "Commands/JamesHarden/Edward/qPositionSetpoint", qProfiledPIDController.getSetpoint().position); DogLog.log( - "EdwardCalculation/qProfiledPID/CurrentVelocityMeasurement", + "Commands/JamesHarden/Edward/qVelocityMeasurement", getDirectionalChassisSpeeds(travelAngle)); DogLog.log( - "EdwardCalculation/qProfiledPID/VelocitySetpoint", + "Commands/JamesHarden/Edward/qVelocitySetpoint", qProfiledPIDController.getSetpoint().velocity); DogLog.log( - "EdwardCalculation/qProfiledPID/PositionError", qProfiledPIDController.getPositionError()); + "Commands/JamesHarden/Edward/qPositionError", qProfiledPIDController.getPositionError()); DogLog.log( - "EdwardCalculation/qProfiledPID/VelocityError", qProfiledPIDController.getVelocityError()); + "Commands/JamesHarden/Edward/qVelocityError", qProfiledPIDController.getVelocityError()); DogLog.log( - "EdwardCalculation/headingProfiledPID/CurrentPositionMeasurement", + "Commands/JamesHarden/Rotational/PositionMeasurement", currentState.Pose.getRotation().getRadians()); DogLog.log( - "EdwardCalculation/headingProfiledPID/PositionSetpoint", + "Commands/JamesHarden/Rotational/PositionSetpoint", headingProfiledPIDController.getSetpoint().position); DogLog.log( - "EdwardCalculation/headingProfiledPID/CurrentVelocityMeasurement", + "Commands/JamesHarden/Rotational/VelocityMeasurement", currentState.Speeds.omegaRadiansPerSecond); DogLog.log( - "EdwardCalculation/headingProfiledPID/VelocitySetpoint", + "Commands/JamesHarden/Rotational/VelocitySetpoint", headingProfiledPIDController.getSetpoint().velocity); DogLog.log( - "EdwardCalculation/headingProfiledPID/PositionError", + "Commands/JamesHarden/Rotational/PositionError", headingProfiledPIDController.getPositionError()); DogLog.log( - "EdwardCalculation/headingProfiledPID/VelocityError", + "Commands/JamesHarden/Rotational/VelocityError", headingProfiledPIDController.getVelocityError()); return new ChassisSpeeds( From f4c83df35dfe07651ac10cad90402794143f26fa Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 13:21:28 -0700 Subject: [PATCH 34/43] reorg robot.java logging --- src/main/java/frc/robot/Robot.java | 36 ++++++++++++++---------------- 1 file changed, 17 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 893d398b..b7b7c3ac 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -66,8 +66,6 @@ public Robot() { */ @Override public void robotPeriodic() { - DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel()); - DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide()); LoggedTalonFX.periodic_static(); CommandScheduler.getInstance().run(); m_robotContainer.doTelemetry(); @@ -90,23 +88,23 @@ public void disabledInit() { public void robotInit() { DogLog.setOptions( new DogLogOptions().withNtPublish(true).withCaptureDs(true).withLogExtras(true)); - DogLog.log("PIDArmKP", Constants.Arm.S0C_KP); - DogLog.log("PIDArmKI", Constants.Arm.S0C_KI); - DogLog.log("PIDArmKD", Constants.Arm.S0C_KD); - DogLog.log("PIDArmKS", Constants.Arm.S0C_KS); - DogLog.log("PIDArmKG", Constants.Arm.S0C_KG); - - DogLog.log("PIDElevatorKP", Constants.ElevatorConstants.S0C_KP); - DogLog.log("PIDElevatorKI", Constants.ElevatorConstants.S0C_KI); - DogLog.log("PIDElevatorKD", Constants.ElevatorConstants.S0C_KD); - DogLog.log("PIDElevatorKS", Constants.ElevatorConstants.S0C_KS); - DogLog.log("PIDElevatorKG", Constants.ElevatorConstants.S0C_KG); - - DogLog.log("PIDTootsieKP", Constants.TootsieSlide.S0C_KP); - DogLog.log("PIDTootsieKI", Constants.TootsieSlide.S0C_KI); - DogLog.log("PIDTootsieKD", Constants.TootsieSlide.S0C_KD); - DogLog.log("PIDTootsieKS", Constants.TootsieSlide.S0C_KS); - DogLog.log("PIDTootsieKG", Constants.TootsieSlide.S0C_KG); + DogLog.log("PIDValues/ArmKP", Constants.Arm.S0C_KP); + DogLog.log("PIDValues/ArmKI", Constants.Arm.S0C_KI); + DogLog.log("PIDValues/ArmKD", Constants.Arm.S0C_KD); + DogLog.log("PIDValues/ArmKS", Constants.Arm.S0C_KS); + DogLog.log("PIDValues/ArmKG", Constants.Arm.S0C_KG); + + DogLog.log("PIDValues/ElevatorKP", Constants.ElevatorConstants.S0C_KP); + DogLog.log("PIDValues/ElevatorKI", Constants.ElevatorConstants.S0C_KI); + DogLog.log("PIDValues/ElevatorKD", Constants.ElevatorConstants.S0C_KD); + DogLog.log("PIDValues/ElevatorKS", Constants.ElevatorConstants.S0C_KS); + DogLog.log("PIDValues/ElevatorKG", Constants.ElevatorConstants.S0C_KG); + + DogLog.log("PIDValues/TootsieKP", Constants.TootsieSlide.S0C_KP); + DogLog.log("PIDValues/TootsieKI", Constants.TootsieSlide.S0C_KI); + DogLog.log("PIDValues/TootsieKD", Constants.TootsieSlide.S0C_KD); + DogLog.log("PIDValues/TootsieKS", Constants.TootsieSlide.S0C_KS); + DogLog.log("PIDValues/TootsieKG", Constants.TootsieSlide.S0C_KG); // Commented this code that logs the electric data because it crashed the robot code // there is an error related to the usage of this // DogLog.setPdh(new PowerDistribution()); From 00b7c6b126498fba3da45668d74a911e8689cfcb Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 13:34:39 -0700 Subject: [PATCH 35/43] logged harden constants --- src/main/java/frc/robot/Constants.java | 18 ++++++++++++ src/main/java/frc/robot/Robot.java | 20 +++++++++++++ .../frc/robot/subsystems/SwerveSubsystem.java | 29 +++++++++---------- 3 files changed, 52 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 477d98c8..6e8464ca 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -46,6 +46,24 @@ public static class Kalman { } public static class HardenConstants { + public static final double QKP = 3.4; + public static final double QKI = 0.45; + public static final double QKD = 0.0005; + public static final double QCRUISE = 4.368; + public static final double QACCEL = 8.0; + public static final double QIZONE = 0.35; + public static final double QIRANGE_LOWER = 0.0; + public static final double QIRANGE_UPPER = 0.2; + + public static final double HKP = 3.7; + public static final double HKI = 0.4; + public static final double HKD = 0.0; + public static final double HCRUISE = 9.417; + public static final double HACCEL = 10.971; + public static final double HIZONE = 0.14; + public static final double HIRANGE_LOWER = 0.0; + public static final double HIRANGE_UPPER = Math.PI / 4.0; + public static class EndWhenCloseEnough { public static final double translationalToleranceTeleop = 0.8d; // 0.43105229381 worked before public static final double translationalToleranceAuto = 1d; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b7b7c3ac..0754a4e7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -105,6 +105,26 @@ public void robotInit() { DogLog.log("PIDValues/TootsieKD", Constants.TootsieSlide.S0C_KD); DogLog.log("PIDValues/TootsieKS", Constants.TootsieSlide.S0C_KS); DogLog.log("PIDValues/TootsieKG", Constants.TootsieSlide.S0C_KG); + + // Q set + DogLog.log("PIDValues/QKP", Constants.HardenConstants.QKP); + DogLog.log("PIDValues/QKI", Constants.HardenConstants.QKI); + DogLog.log("PIDValues/QKD", Constants.HardenConstants.QKD); + DogLog.log("PIDValues/QCRUISE", Constants.HardenConstants.QCRUISE); + DogLog.log("PIDValues/QACCEL", Constants.HardenConstants.QACCEL); + DogLog.log("PIDValues/QIZONE", Constants.HardenConstants.QIZONE); + DogLog.log("PIDValues/QIRANGE_LOWER", Constants.HardenConstants.QIRANGE_LOWER); + DogLog.log("PIDValues/QIRANGE_UPPER", Constants.HardenConstants.QIRANGE_UPPER); + + // H set + DogLog.log("PIDValues/HKP", Constants.HardenConstants.HKP); + DogLog.log("PIDValues/HKI", Constants.HardenConstants.HKI); + DogLog.log("PIDValues/HKD", Constants.HardenConstants.HKD); + DogLog.log("PIDValues/HCRUISE", Constants.HardenConstants.HCRUISE); + DogLog.log("PIDValues/HACCEL", Constants.HardenConstants.HACCEL); + DogLog.log("PIDValues/HIZONE", Constants.HardenConstants.HIZONE); + DogLog.log("PIDValues/HIRANGE_LOWER", Constants.HardenConstants.HIRANGE_LOWER); + DogLog.log("PIDValues/HIRANGE_UPPER", Constants.HardenConstants.HIRANGE_UPPER); // Commented this code that logs the electric data because it crashed the robot code // there is an error related to the usage of this // DogLog.setPdh(new PowerDistribution()); diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index c91243a1..51f1f4ff 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -69,22 +69,21 @@ public SwerveSubsystem( // 1.7, 0.345, 0.0015 qProfiledPIDController = new ProfiledPIDController( - 3.4, // 3.4 not bad // [3.4 good for 0.2-1.2, 0.425 I] - 0.45, // 345 - 0.0005, // 0.0015 + Constants.HardenConstants.QKP, // 3.4 not bad // [3.4 good for 0.2-1.2, 0.425 I] + Constants.HardenConstants.QKI, // 345 + Constants.HardenConstants.QKD, // 0.0015 new TrapezoidProfile.Constraints( - Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND - 0.5, - 8)); // 8.25 // 5 accel and 0.75 p was good + Constants.HardenConstants.QCRUISE, + Constants.HardenConstants.QACCEL)); // 8.25 // 5 accel and 0.75 p was good headingProfiledPIDController = new ProfiledPIDController( - 3.7, // 4 was good - 0.4, // - 0, + Constants.HardenConstants.HKP, // 4 was good + Constants.HardenConstants.HKI, // + Constants.HardenConstants.HKD, new TrapezoidProfile.Constraints( - Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_RATE - 1.5, // -1 was good - Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_ACCELERATION_UNITS_PER_SECOND - - 16)); // -13 was good + Constants.HardenConstants.HCRUISE, // -1 was good + Constants.HardenConstants.HACCEL)); // -13 was good // headingProfiledPIDController = // new ProfiledPIDController( // 1, // 4 was good @@ -95,11 +94,11 @@ public SwerveSubsystem( // Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_ACCELERATION_UNITS_PER_SECOND // - 16)); // -13 was good - qProfiledPIDController.setIZone(0.35); - headingProfiledPIDController.setIZone(0.14); + qProfiledPIDController.setIZone(Constants.HardenConstants.QIZONE); + headingProfiledPIDController.setIZone(Constants.HardenConstants.HIZONE); - qProfiledPIDController.setIntegratorRange(0, 0.2); - headingProfiledPIDController.setIntegratorRange(0.0, Math.PI / 4); // 0.3 before + qProfiledPIDController.setIntegratorRange(Constants.HardenConstants.QIRANGE_LOWER, Constants.HardenConstants.QIRANGE_UPPER); + headingProfiledPIDController.setIntegratorRange(Constants.HardenConstants.HIRANGE_LOWER, Constants.HardenConstants.HIRANGE_UPPER); // 0.3 before headingProfiledPIDController.enableContinuousInput(-Math.PI, Math.PI); // configureAutoBuilder(); From 148ccd07891a2586c2896ae000e0328f1d862387 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 13:36:35 -0700 Subject: [PATCH 36/43] robot container logging fixed --- src/main/java/frc/robot/RobotContainer.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 404e2792..b024aee0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,7 +83,6 @@ public void doTelemetry() { if (driveTrain.getCurrentCommand() != null) { commandName = driveTrain.getCurrentCommand().getName(); } - DogLog.log("Robot/SwerveDriveCommand", commandName); } public RobotContainer() { @@ -471,7 +470,7 @@ public static void setAlliance() { } public BooleanSupplier getRedSide() { - DogLog.log("get alliance", redside.getAsBoolean()); + DogLog.log("Info/Alliance", redside.getAsBoolean()); return redside; } @@ -479,7 +478,7 @@ public Command getAutonomousCommand() { /* Run the path selected from the auto chooser */ int autoValue = autoChooser.getSelected(); Command autoCommand; - DogLog.log("auto/selected", autoValue); + DogLog.log("Auto/Selected", autoValue); switch (autoValue) { case 1: autoCommand = From 4309da775ba52a90729f211764ee16ddff86f3e8 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 13:41:58 -0700 Subject: [PATCH 37/43] fixed logging until ZeroElevatorHardStop (non-inclusive) --- src/main/java/frc/robot/AutoRoutines/AutoProducer.java | 2 +- src/main/java/frc/robot/commandGroups/JamesHardenScore.java | 4 +--- .../commands/DaleCommands/ArmToAngleAndSpinFlywheel.java | 2 +- .../java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java | 2 +- src/main/java/frc/robot/commands/EndWhenCloseEnough.java | 4 ++-- 5 files changed, 6 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/AutoRoutines/AutoProducer.java b/src/main/java/frc/robot/AutoRoutines/AutoProducer.java index 9f1b4256..136263e2 100644 --- a/src/main/java/frc/robot/AutoRoutines/AutoProducer.java +++ b/src/main/java/frc/robot/AutoRoutines/AutoProducer.java @@ -93,7 +93,7 @@ private void settyCycle( JamesHardenMovement movementCommand, maintainCommand; if (!scorePosition.isBranch()) { - DogLog.log("JamesHardenScore/Errors", "called without real branch"); + DogLog.log("Commands/JamesHardenScore/Errors", "Called without a real branch"); return; } movementCommand = JamesHardenMovement.toSpecificBranch(driveTrain, () -> scorePosition, false); diff --git a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java index 37e05084..3541c471 100644 --- a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java +++ b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java @@ -72,7 +72,7 @@ public JamesHardenScore( JamesHardenMovement movementCommand, maintainCommand; if (!branch.isBranch()) { - DogLog.log("JamesHardenScore/Errors", "called without real branch"); + DogLog.log("Commands/JamesHardenScore/Errors", "Called without a real branch"); return; } @@ -85,8 +85,6 @@ public JamesHardenScore( } else { elevateCommand = new SetElevatorLevel(elevatorSubsystem, height, true); } - // if (DriverStation.isTeleop()) { - DogLog.log("JamesHardenScore/Version", "teleop"); addCommands( movementCommand.alongWith( diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java index a5d410af..c0c79071 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java @@ -27,7 +27,7 @@ public void initialize() {} @Override public void execute() { armPlusFlywheel.spinFlywheel(); // Spin the flywheel at desired speed - DogLog.log("Running to target angle: ", angle); + DogLog.log("Commands/ArmToAngleAndSpinFlywheel/TargetAngle", angle); armPlusFlywheel.setPosition(angle); } diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java index 478deada..801b3c82 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java @@ -21,7 +21,7 @@ public void initialize() {} @Override public void execute() { - DogLog.log("Running to target angle: ", angle); + DogLog.log("Commands/ArmToAngleCmd/TargetAngle", angle); arm.setPosition(angle); } diff --git a/src/main/java/frc/robot/commands/EndWhenCloseEnough.java b/src/main/java/frc/robot/commands/EndWhenCloseEnough.java index 9aebf3a2..2b0c097e 100644 --- a/src/main/java/frc/robot/commands/EndWhenCloseEnough.java +++ b/src/main/java/frc/robot/commands/EndWhenCloseEnough.java @@ -34,7 +34,7 @@ public EndWhenCloseEnough( // Called when the command is initially scheduled. @Override public void initialize() { - DogLog.log("END_WHEN_CLOSE_ENOUGH", true); + DogLog.log("Commands/CloseEnough", false); } // Called every time the scheduler runs while the command is scheduled. @@ -44,7 +44,7 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DogLog.log("END_WHEN_CLOSE_ENOUGH", false); + DogLog.log("Commands/CloseEnough", true); } // Returns true when the command should end. From 2c0a3cc9f341b03c58cef981dd4fd051d3253c1d Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 14:21:15 -0700 Subject: [PATCH 38/43] logging fixes --- .../ZeroElevatorHardStop.java | 10 +- .../ZeroElevatorToFFiltered.java | 10 +- .../FunnelCommands/CoralCheckedIn.java | 6 +- .../SwerveCommands/ApplySwerveVoltage.java | 1 - .../SwerveCommands/JamesHardenMovement.java | 38 +- .../SwerveCommands/SwerveJoystickCommand.java | 8 +- .../frc/robot/subsystems/VisionSystem.java | 704 ------------------ 7 files changed, 34 insertions(+), 743 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/VisionSystem.java diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java index c19e9668..aaedf985 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java @@ -10,7 +10,7 @@ public class ZeroElevatorHardStop extends Command { public ZeroElevatorHardStop(ElevatorSubsystem subsystem) { elevatorSubsystem = subsystem; - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false); + DogLog.log("Commands/ZeroElevatorHardStop/Running", false); addRequirements(elevatorSubsystem); } @@ -18,7 +18,7 @@ public ZeroElevatorHardStop(ElevatorSubsystem subsystem) { @Override public void initialize() { elevatorSubsystem.reduceCurrentLimits(); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", true); + DogLog.log("Commands/ZeroElevatorHardStop/Running", true); timesExceededCurrent = 0; } @@ -32,7 +32,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false); + DogLog.log("Commands/ZeroElevatorHardStop/Running", false); if (!interrupted) { elevatorSubsystem.resetElevatorPositionToZero(); } @@ -43,9 +43,9 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/timesExceededCount", timesExceededCurrent); + DogLog.log("Commands/ZeroElevatorHardStop/timesExceededCurrent", timesExceededCurrent); boolean checkCurrent = elevatorSubsystem.checkCurrent(); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/checkcurrent", checkCurrent); + DogLog.log("Commands/ZeroElevatorHardStop/checkCurrent", checkCurrent); if (checkCurrent) { timesExceededCurrent++; } else { diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java index 4b265808..28cfe065 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java @@ -11,14 +11,14 @@ public class ZeroElevatorToFFiltered extends Command { public ZeroElevatorToFFiltered(ElevatorSubsystem subsystem) { elevatorSubsystem = subsystem; - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", false); addRequirements(elevatorSubsystem); } @Override public void initialize() { ticksAtPosition = 0; - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", true); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", true); elevatorSubsystem.elevateTo(ElevatorPositions.Intake); } @@ -27,15 +27,15 @@ public void execute() {} @Override public void end(boolean interrupted) { - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", false); elevatorSubsystem.resetPositionFiltered(); } @Override public boolean isFinished() { - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition); + DogLog.log("Commands/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition); boolean inPosition = elevatorSubsystem.isAtPosition() && elevatorSubsystem.atIntake(); - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/inPosition", inPosition); + DogLog.log("Commands/ZeroElevatorToFFiltered/inPosition", inPosition); if (inPosition) { ticksAtPosition++; diff --git a/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java b/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java index 0420890c..af88577d 100644 --- a/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java +++ b/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java @@ -9,13 +9,13 @@ public class CoralCheckedIn extends Command { public CoralCheckedIn(FunnelSubsystem funnelSubsystem) { this.funnelSubsystem = funnelSubsystem; - DogLog.log("CoralCheckedIn", false); + DogLog.log("CoralPosition/CoralCheckedIn", false); } // Called when the command is initially scheduled. @Override public void initialize() { - DogLog.log("CoralCheckedIn", true); + DogLog.log("CoralPosition/CoralCheckedIn", true); // Store the position of the coral when it was first checked out. } @@ -27,7 +27,7 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DogLog.log("CoralCheckedIn", false); + DogLog.log("CoralPosition/CoralCheckedIn", false); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java index ca4cc04b..3e0870ce 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java @@ -25,7 +25,6 @@ public void initialize() { @Override public void execute() { - DogLog.log("restarted", counter); swerveDrivetrain.getModule(0).getDriveMotor().setVoltage(0.2425 + applyVoltage); swerveDrivetrain.getModule(1).getDriveMotor().setVoltage(0.2425 + applyVoltage); swerveDrivetrain.getModule(2).getDriveMotor().setVoltage(0.2425 + applyVoltage); diff --git a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java index 334ec461..93a12da4 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java @@ -99,21 +99,21 @@ public void execute() { ChassisSpeeds speeds = swerve.calculateRequiredEdwardChassisSpeeds(targetPose, initialPathDistance); - DogLog.log("JamesHardenMovement/TargetPose", targetPose); - DogLog.log("JamesHardenMovement/TargetPoseX(m)", targetPose.getX()); - DogLog.log("JamesHardenMovement/TargetPoseY(m)", targetPose.getY()); - DogLog.log("JamesHardenMovement/TargetPoseHeading(deg)", targetPose.getRotation().getRadians()); + DogLog.log("Commands/JamesHarden/TargetPose", targetPose); + DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseX(m)", targetPose.getX()); + DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseY(m)", targetPose.getY()); + DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseHeading(deg)", targetPose.getRotation().getRadians()); - DogLog.log("JamesHardenMovement/DesiredChassisSpeedsX(mps)", speeds.vxMetersPerSecond); - DogLog.log("JamesHardenMovement/DesiredChassisSpeedsY(mps)", speeds.vyMetersPerSecond); - DogLog.log("JamesHardenMovement/DesiredChassisSpeedsX(radps)", speeds.omegaRadiansPerSecond); + DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsX(mps)", speeds.vxMetersPerSecond); + DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsY(mps)", speeds.vyMetersPerSecond); + DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsTheta(radps)", speeds.omegaRadiansPerSecond); DogLog.log( - "JamesHardenMovement/ActualChassisSpeedsX(mps)", swerve.getFieldSpeeds().vxMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)", swerve.getFieldSpeeds().vxMetersPerSecond); DogLog.log( - "JamesHardenMovement/ActualChassisSpeedsY(mps)", swerve.getFieldSpeeds().vyMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)", swerve.getFieldSpeeds().vyMetersPerSecond); DogLog.log( - "JamesHardenMovement/ActualChassisSpeedsX(radps)", + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(radps)", swerve.getFieldSpeeds().omegaRadiansPerSecond); swerve.setFieldSpeeds(speeds); } @@ -127,21 +127,21 @@ public boolean isFinished() { currRot = ((2.0 * Math.PI) + (currRot % (2.0 * Math.PI))) % (2.0 * Math.PI); double targetRot = targetPose.getRotation().getRadians(); targetRot = ((2.0 * Math.PI) + (targetRot % (2.0 * Math.PI))) % (2.0 * Math.PI); - DogLog.log("JamesHardenMovement/currRot(rad)", currRot); - DogLog.log("JamesHardenMovement/targetRot(rad)", targetRot); + DogLog.log("Commands/JamesHarden/IsFinished/currRot(rad)", currRot); + DogLog.log("Commands/JamesHarden/IsFinished/targetRot(rad)", targetRot); DogLog.log( - "JamesHardenMovement/xTolMet", + "Commands/JamesHarden/IsFinished/xTolMet", (Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) < Constants.HardenConstants.RegularCommand.xyIndividualTolerance)); DogLog.log( - "JamesHardenMovement/yTolMet", + "Commands/JamesHarden/IsFinished/yTolMet", (Math.abs(swerve.getCurrentState().Pose.getY() - targetPose.getY()) < Constants.HardenConstants.RegularCommand.xyIndividualTolerance)); DogLog.log( - "JamesHardenMovement/rotTolMet", + "Commands/JamesHarden/IsFinished/rotTolMet", (Math.min(Math.abs(targetRot - currRot), (Math.PI * 2) - Math.abs(targetRot - currRot)) < Constants.HardenConstants.RegularCommand.headingTolerance)); - DogLog.log("JamesHardenMovement/End", false); + DogLog.log("Commands/JamesHarden/End", false); // translational is met if ((Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) @@ -170,7 +170,7 @@ public boolean isFinished() { + (swerve.getFieldSpeeds().vyMetersPerSecond * swerve.getFieldSpeeds().vyMetersPerSecond)) <= 0.2)) { - DogLog.log("JamesHardenMovement/End", true); + DogLog.log("Commands/JamesHarden/End", true); return true; } else return false; } @@ -258,10 +258,6 @@ public static JamesHardenMovement toClosestRightBranch( Supplier targetBranch = () -> { Translation2d currPosition = swerve.getCurrentState().Pose.getTranslation(); - DogLog.log("currPositionX", currPosition.getX()); - DogLog.log("currPositionY", currPosition.getY()); - DogLog.log("currPositionTheta", currPosition.getAngle().getRadians()); - DogLog.log("hardenRed", redSide.getAsBoolean()); if (redSide.getAsBoolean()) { double minDist = currPosition.getDistance(RedLandmarkPose.R0.getPose().getTranslation()); diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java index ebe874fe..e9a02bb1 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java @@ -193,10 +193,10 @@ public void execute() { final double y = ySpeed; double turn = turningSpeed; - DogLog.log("joystickCommand/xSpeed", xSpeed); - DogLog.log("joystickCommand/ySpeed", ySpeed); - DogLog.log("joystickCommand/turningSpeed", turningSpeed); - DogLog.log("fieldCentric", fieldRelativeFunction.getAsBoolean()); + DogLog.log("Commands/joystickCommand/xSpeed", xSpeed); + DogLog.log("Commands/joystickCommand/ySpeed", ySpeed); + DogLog.log("Commands/joystickCommand/turningSpeed", turningSpeed); + DogLog.log("Information/fieldCentric", fieldRelativeFunction.getAsBoolean()); // 5. Applying the drive request on the swerve drivetrain // Uses SwerveRequestFieldCentric (from java.frc.robot.util to apply module optimization) if (fixedRotation.getAsBoolean()) { diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java deleted file mode 100644 index 2d937f3d..00000000 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ /dev/null @@ -1,704 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import dev.doglog.DogLog; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; -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.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import java.util.List; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import java.util.stream.Collectors; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -/** - * VisionSystem fuses multi-tag AprilTag measurements from PhotonVision with swerve odometry by - * dynamically computing measurement noise based on tag count, distance, viewing angle, and robot - * speed. - */ -public class VisionSystem extends SubsystemBase { - private static VisionSystem[] systemList = - new VisionSystem[Constants.Vision.Cameras.values().length]; - - private final Constants.Vision.Cameras cameraId; - // Reef tag IDs for each side of the field - private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); - private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); - - // Base noise tuning parameters (tweakable) - private double calibrationFactor = 1.0; - private double baseNoiseX = 0.01; // meters - private double baseNoiseY = 0.01; - private double baseNoiseTheta = 0.5; // radians - - private double distanceCoefficientX = 0.055; // noise growth per meter - private double distanceCoefficientY = 0.055; - private double distanceCoefficientTheta = 1; - - private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle - private double angleCoefficientY = 0.5; - private double angleCoefficientTheta = 0.5; - - private double speedCoefficientX = 0.5; // noise growth per fraction of max speed - private double speedCoefficientY = 0.5; - private double speedCoefficientTheta = 0.5; - - // Maximums for normalization - private double maximumViewingAngle = Math.toRadians(90.0); - private double maximumRobotSpeed = 5; // meters per second - private double maximumAllowedDistance = 15.0; // meters, beyond which readings are dropped - - // PhotonVision and odometry references - private final PhotonCamera photonCamera; - private final PhotonPoseEstimator poseEstimator; // MULTI_TAG_PNP_ON_COPROCESSOR - private final PhotonPoseEstimator trigsolvePoseEstimator; // PNP_DISTANCE_TRIG_SOLVE - private PhotonPipelineResult latestVisionResult; - private final BooleanSupplier isRedSide; - private Pose2d lastKnownPose = new Pose2d(0, 0, new Rotation2d()); - private Pose2d lastKnownTrigsolvePose = new Pose2d(0, 0, new Rotation2d()); - private SwerveSubsystem swerveDrive; - private final AprilTagFieldLayout fieldLayout; - - public VisionSystem(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { - this.isRedSide = isRedSide; - this.cameraId = cameraId; - photonCamera = new PhotonCamera(cameraId.toString()); - Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); - - // Initialize field layout with error handling - AprilTagFieldLayout tempLayout = null; - try { - tempLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - } catch (Exception e) { - System.err.println("Failed to load AprilTag field layout: " + e.getMessage()); - e.printStackTrace(); - } - this.fieldLayout = tempLayout; - - // Initialize both pose estimators only if field layout loaded successfully - if (fieldLayout != null) { - poseEstimator = - new PhotonPoseEstimator( - fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); - - trigsolvePoseEstimator = - new PhotonPoseEstimator(fieldLayout, PoseStrategy.PNP_DISTANCE_TRIG_SOLVE, cameraToRobot); - } else { - poseEstimator = null; - trigsolvePoseEstimator = null; - } - - latestVisionResult = null; - - // Minimal debug logging (guarded by ENABLE_LOGS) - v("Init", true); - v("FieldLayoutLoaded", fieldLayout != null); - } - - public static VisionSystem getInstance( - Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { - int idx = cameraId.ordinal(); - if (systemList[idx] == null) { - systemList[idx] = new VisionSystem(cameraId, isRedSide); - } - return systemList[idx]; - } - - @Override - public void periodic() { - // Initialize swerve drive if not already done - if (swerveDrive == null) { - try { - swerveDrive = SwerveSubsystem.getInstance(); - } catch (Exception e) { - v("SwerveNotReady", true); - return; - } - } - - // Log that periodic is running (guarded) - v("Periodic", true); - - // Check camera connection - boolean cameraConnected = photonCamera.isConnected(); - v("CameraConnected", cameraConnected); - - if (!cameraConnected) { - v("CameraDisconnected", true); - return; - } - - // Get all unread results - var results = photonCamera.getAllUnreadResults(); - v("UnreadResultsCount", results.size()); - - for (var result : results) { - latestVisionResult = result; - v("ResultTimestamp", result.getTimestampSeconds()); - v("HasTargets", result.hasTargets()); - if (result.hasTargets()) { - v("TargetCount", result.getTargets().size()); - } - } - - // If we have a recent result, try to add it - if (latestVisionResult != null) { - double timeSinceResult = Timer.getFPGATimestamp() - latestVisionResult.getTimestampSeconds(); - v("TimeSinceLastResult", timeSinceResult); - - // Only use results that are recent (within 0.5 seconds) - if (timeSinceResult < 0.5) { - addFilteredPose(); - } - } - } - - /** - * Attempts to fuse a vision measurement into the swerve pose estimator using MULTI_TAG_PNP - * strategy, dropping readings that fail validity checks, and computing noise dynamically via - * computeMeasurementNoise(). - */ - public void addFilteredPose() { - addFilteredPose(false); - } - - /** - * Attempts to fuse a vision measurement into the swerve pose estimator, dropping readings that - * fail validity checks, and computing noise dynamically via computeMeasurementNoise(). - * - * @param useTrigsolve If true, uses the trigsolve pose estimator (LOWEST_AMBIGUITY strategy). If - * false, uses the multi-tag PnP pose estimator (MULTI_TAG_PNP_ON_COPROCESSOR strategy). - */ - public void addFilteredPose(boolean useTrigsolve) { - addFilteredPoseInternal(useTrigsolve, false); - } - - /** - * Internal method to handle pose estimation with optional confidence checking. - * - * @param useTrigsolve If true, uses trigsolve strategy - * @param forceAdd If true, bypasses confidence checking (used for non-trigsolve or single camera) - * @return PoseEstimateResult containing the pose and confidence data, or null if failed - */ - private PoseEstimateResult addFilteredPoseInternal(boolean useTrigsolve, boolean forceAdd) { - final String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; - - // Check prerequisites - if (fieldLayout == null) { - v(strategyPrefix + "/FieldLayoutMissing", true); - return null; - } - - if (swerveDrive == null) { - v(strategyPrefix + "/SwerveDriveNull", true); - return null; - } - - PhotonPoseEstimator selectedEstimator = useTrigsolve ? trigsolvePoseEstimator : poseEstimator; - if (selectedEstimator == null) { - v(strategyPrefix + "/PoseEstimatorNull", true); - return null; - } - - if (latestVisionResult == null || !latestVisionResult.hasTargets()) { - if (!forceAdd) { - v(strategyPrefix + "/HasTargets", false); - vts(strategyPrefix + "/Frame/NoTargets"); - } - return null; - } - v(strategyPrefix + "/HasTargets", true); - vts(strategyPrefix + "/Frame/Start"); - - // Filter tags to only those on the active side - List validTags = - latestVisionResult.getTargets().stream() - .filter(t -> isTagOnActiveSide(t.getFiducialId())) - .collect(Collectors.toList()); - - // Log all detected tags for debugging - String allTagIds = - latestVisionResult.getTargets().stream() - .map(t -> Integer.toString(t.getFiducialId())) - .collect(Collectors.joining(",")); - v(strategyPrefix + "/AllDetectedTags", allTagIds); - v(strategyPrefix + "/IsRedSide", isRedSide.getAsBoolean()); - - if (validTags.isEmpty()) { - if (!forceAdd) { - v(strategyPrefix + "/TagFilter", false); - } - return null; - } - - int tagCount = validTags.size(); - v(strategyPrefix + "/Tags/Count", tagCount); - String tagIdsCsv = - validTags.stream() - .map(t -> Integer.toString(t.getFiducialId())) - .sorted() - .collect(Collectors.joining(",")); - v(strategyPrefix + "/Tags/IDs", tagIdsCsv); - - // Compute effective metrics for solution - // Use camera→target distance from PV (avoids odometry dependence) - double averageDistance = - validTags.stream() - .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) - .average() - .orElse(Double.NaN); - double minDistance = - validTags.stream() - .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) - .min() - .orElse(Double.NaN); - v(strategyPrefix + "/Range/AvgM", averageDistance); - v(strategyPrefix + "/Range/MinM", minDistance); - if (Double.isNaN(minDistance) || minDistance > maximumAllowedDistance) { - if (!forceAdd) { - v(strategyPrefix + "/Gate/DistanceRejected", true); - v(strategyPrefix + "/Range/MaxAllowedM", maximumAllowedDistance); - } - return null; - } - // Compute true 3D viewing skew for each tag: angle between camera +X and NEGATED tag +Z - double minSkewRad = - validTags.stream().mapToDouble(this::computeTargetSkewRad).min().orElse(0.0); - double maxSkewRad = - validTags.stream().mapToDouble(this::computeTargetSkewRad).max().orElse(0.0); - double rmsSkewRad = computeWeightedRmsSkewRad(validTags); - double avgSkewRad = - validTags.stream().mapToDouble(this::computeTargetSkewRad).average().orElse(0.0); - - v(strategyPrefix + "/Skew/MinDeg", Math.toDegrees(minSkewRad)); - v(strategyPrefix + "/Skew/AvgDeg", Math.toDegrees(avgSkewRad)); - v(strategyPrefix + "/Skew/RmsDeg", Math.toDegrees(rmsSkewRad)); - v(strategyPrefix + "/Skew/MaxDeg", Math.toDegrees(maxSkewRad)); - - // No skew gating: skew only affects noise scaling below. - - // Use strategy-appropriate skew for noise scaling - double averageAngle = useTrigsolve ? minSkewRad : rmsSkewRad; - double currentSpeed = 0.0; - try { - currentSpeed = - Math.hypot( - swerveDrive.getRobotSpeeds().vxMetersPerSecond, - swerveDrive.getRobotSpeeds().vyMetersPerSecond); - } catch (Exception e) { - v(strategyPrefix + "/SpeedCalculationError", true); - } - - v(strategyPrefix + "/Inputs/SkewUsedDeg", Math.toDegrees(averageAngle)); - v(strategyPrefix + "/Inputs/SpeedMps", currentSpeed); - - // Choose the appropriate pose estimator and reference pose - Pose2d referencePos = useTrigsolve ? lastKnownTrigsolvePose : lastKnownPose; - - // Update reference pose with current odometry if we have it - try { - Pose2d currentOdometry = swerveDrive.getPose(); - if (currentOdometry != null) { - selectedEstimator.setReferencePose(currentOdometry); - v(strategyPrefix + "/ReferencePose/Pose", currentOdometry); - } else { - selectedEstimator.setReferencePose(referencePos); - } - } catch (Exception e) { - v(strategyPrefix + "/ReferencePoseError", true); - selectedEstimator.setReferencePose(referencePos); - } - - // Get the pose from PhotonVision - Optional maybePose = selectedEstimator.update(latestVisionResult); - if (maybePose.isEmpty()) { - if (!forceAdd) { - v(strategyPrefix + "/Pose/EstimateFailed", true); - vts(strategyPrefix + "/Frame/End"); - } - return null; - } - - EstimatedRobotPose estimatedPose = maybePose.get(); - Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); - - // Calculate confidence metric for trigsolve based on measurement quality - double confidence = 1.0; // Default confidence for multi-tag - if (useTrigsolve) { - // For trigsolve, calculate confidence based on the measurement noise multipliers - // Lower total noise = higher confidence - NoiseComponents nxC = - computeNoiseComponents( - baseNoiseX, - distanceCoefficientX, - angleCoefficientX, - speedCoefficientX, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - NoiseComponents nyC = - computeNoiseComponents( - baseNoiseY, - distanceCoefficientY, - angleCoefficientY, - speedCoefficientY, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - NoiseComponents nthC = - computeNoiseComponents( - baseNoiseTheta, - distanceCoefficientTheta, - angleCoefficientTheta, - speedCoefficientTheta, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - - double noiseX = nxC.total; - double noiseY = nyC.total; - double noiseTheta = nthC.total; - - // Unitless blend for confidence - double nxU = noiseX / Math.max(baseNoiseX, 1e-9); - double nyU = noiseY / Math.max(baseNoiseY, 1e-9); - double nthU = noiseTheta / Math.max(baseNoiseTheta, 1e-9); - double totalNoise = Math.sqrt(nxU * nxU + nyU * nyU + nthU * nthU); - confidence = 1.0 / (1.0 + totalNoise); - - // Structured logs for tuning (Trigsolve only) - v(strategyPrefix + "/Confidence", confidence); - v(strategyPrefix + "/Noise/X/Total", noiseX); - v(strategyPrefix + "/Noise/Y/Total", noiseY); - v(strategyPrefix + "/Noise/Theta/Total", noiseTheta); - v(strategyPrefix + "/Noise/TagScale", nxC.tagScale); // same for all axes - v(strategyPrefix + "/Noise/X/DistanceTerm", nxC.distanceTerm); - v(strategyPrefix + "/Noise/X/AngleTerm", nxC.angleTerm); - v(strategyPrefix + "/Noise/X/SpeedTerm", nxC.speedTerm); - v(strategyPrefix + "/Noise/Y/DistanceTerm", nyC.distanceTerm); - v(strategyPrefix + "/Noise/Y/AngleTerm", nyC.angleTerm); - v(strategyPrefix + "/Noise/Y/SpeedTerm", nyC.speedTerm); - v(strategyPrefix + "/Noise/Theta/DistanceTerm", nthC.distanceTerm); - v(strategyPrefix + "/Noise/Theta/AngleTerm", nthC.angleTerm); - v(strategyPrefix + "/Noise/Theta/SpeedTerm", nthC.speedTerm); - } - - // If this is just for confidence comparison, return the result without adding to odometry - if (forceAdd) { - return new PoseEstimateResult( - measuredPose, - confidence, - averageDistance, - averageAngle, - currentSpeed, - tagCount, - latestVisionResult.getTimestampSeconds()); - } - - // Process locally (no cross-camera comparison) - return processPoseEstimate( - measuredPose, - useTrigsolve, - averageDistance, - averageAngle, - currentSpeed, - tagCount, - latestVisionResult.getTimestampSeconds()); - } - - // (processTrigsolveWithConfidenceCheck removed) - - /** Final processing and addition of pose estimate to odometry. */ - private PoseEstimateResult processPoseEstimate( - Pose2d measuredPose, - boolean useTrigsolve, - double averageDistance, - double averageAngle, - double currentSpeed, - int tagCount, - double timestamp) { - - // Compute measurement noise for each axis and log components - NoiseComponents nxC = - computeNoiseComponents( - baseNoiseX, - distanceCoefficientX, - angleCoefficientX, - speedCoefficientX, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - NoiseComponents nyC = - computeNoiseComponents( - baseNoiseY, - distanceCoefficientY, - angleCoefficientY, - speedCoefficientY, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - NoiseComponents nthC = - computeNoiseComponents( - baseNoiseTheta, - distanceCoefficientTheta, - angleCoefficientTheta, - speedCoefficientTheta, - averageDistance, - averageAngle, - currentSpeed, - tagCount); - - double noiseX = nxC.total; - double noiseY = nyC.total; - double noiseTheta = nthC.total; - - String strategyPrefix = useTrigsolve ? "Trigsolve" : "MultiTag"; - v(strategyPrefix + "/Noise/X/Total", noiseX); - v(strategyPrefix + "/Noise/Y/Total", noiseY); - v(strategyPrefix + "/Noise/Theta/Total", noiseTheta); - v(strategyPrefix + "/Noise/TagScale", nxC.tagScale); - v(strategyPrefix + "/Noise/X/DistanceTerm", nxC.distanceTerm); - v(strategyPrefix + "/Noise/X/AngleTerm", nxC.angleTerm); - v(strategyPrefix + "/Noise/X/SpeedTerm", nxC.speedTerm); - v(strategyPrefix + "/Noise/Y/DistanceTerm", nyC.distanceTerm); - v(strategyPrefix + "/Noise/Y/AngleTerm", nyC.angleTerm); - v(strategyPrefix + "/Noise/Y/SpeedTerm", nyC.speedTerm); - v(strategyPrefix + "/Noise/Theta/DistanceTerm", nthC.distanceTerm); - v(strategyPrefix + "/Noise/Theta/AngleTerm", nthC.angleTerm); - v(strategyPrefix + "/Noise/Theta/SpeedTerm", nthC.speedTerm); - - // Update the appropriate reference pose - if (useTrigsolve) { - lastKnownTrigsolvePose = measuredPose; - } else { - lastKnownPose = measuredPose; - } - - // Choose timestamp: use vision timestamp unless it differs too much from FPGA - double fpgaTimestamp = Timer.getFPGATimestamp(); - double timestampDifference = Math.abs(timestamp - fpgaTimestamp); - double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp : timestamp; - - v(strategyPrefix + "/Pose/Pose", measuredPose); - v(strategyPrefix + "/Timestamp/Vision", timestamp); - v(strategyPrefix + "/Timestamp/FPGA", fpgaTimestamp); - v(strategyPrefix + "/Timestamp/Delta", timestampDifference); - - // Build the noise vector and add the vision measurement - Matrix noiseVector = VecBuilder.fill(noiseX, noiseY, noiseTheta); - - try { - swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); - v(strategyPrefix + "/MeasurementUsed", true); - } catch (Exception e) { - v(strategyPrefix + "/MeasurementFailed", true); - v("AddMeasurementError", e.getMessage()); - } - - vts(strategyPrefix + "/Frame/End"); - - return new PoseEstimateResult( - measuredPose, 1.0, averageDistance, averageAngle, currentSpeed, tagCount, timestamp); - } - - private boolean isTagOnActiveSide(int tagId) { - return isRedSide.getAsBoolean() - ? RED_SIDE_TAG_IDS.contains(tagId) - : BLUE_SIDE_TAG_IDS.contains(tagId); - } - - // (getDistanceToTag removed) - - // ─── Vision logging helpers ────────────────────────────────────────────────── - private static final boolean ENABLE_LOGS = true; // flip true when tuning - - private String vKey(String subkey) { - return "Vision/" + cameraId.toString() + "/" + subkey; - } - - private void vts(String subkey) { - if (!ENABLE_LOGS) return; - DogLog.timestamp(vKey(subkey)); - } - - private void v(String subkey, double val) { - if (!ENABLE_LOGS) return; - DogLog.log(vKey(subkey), val); - } - - private void v(String subkey, boolean val) { - if (!ENABLE_LOGS) return; - DogLog.log(vKey(subkey), val); - } - - private void v(String subkey, String val) { - if (!ENABLE_LOGS) return; - DogLog.log(vKey(subkey), val); - } - - private void v(String subkey, Pose2d val) { - if (!ENABLE_LOGS) return; - DogLog.log(vKey(subkey), val); - } - - private static class NoiseComponents { - double tagScale; // 1/sqrt(tags) - double distanceTerm; // base + k*d^2 - double angleTerm; // cosine-based (1 - cos θ) normalized - double speedTerm; // 1 + k*(v/vmax)^2 - double total; // calibrationFactor * tagScale * distanceTerm * angleTerm * speedTerm - } - - private NoiseComponents computeNoiseComponents( - double baseNoise, - double distanceCoefficient, - double angleCoefficient, - double speedCoefficient, - double distance, - double angleRad, - double robotSpeed, - int tagCount) { - NoiseComponents c = new NoiseComponents(); - - // Tag count factor (diminishing returns; cap at 4) - int effectiveTags = Math.max(1, Math.min(tagCount, 4)); - c.tagScale = 1.0 / Math.sqrt(effectiveTags); - - // Distance term (keep as d^2 per your model) - c.distanceTerm = baseNoise + distanceCoefficient * distance * distance; - - // Angle term (cosine-based): scale with 1 - cos(theta), normalized to cutoff - double theta = Math.max(0.0, Math.min(angleRad, 0.999 * maximumViewingAngle)); - double cosT = Math.cos(theta); - double cosMax = Math.cos(0.999 * maximumViewingAngle); - double denom = Math.max(1.0 - cosMax, 1e-6); - double normalizedAngle = (1.0 - cosT) / denom; // 0 at head-on, 1 at max angle - normalizedAngle = Math.max(0.0, Math.min(normalizedAngle, 1.0)); - c.angleTerm = 1.0 + angleCoefficient * normalizedAngle; - - // Speed term (quadratic, saturated) - double vNorm = - Math.max(0.0, Math.min(robotSpeed, maximumRobotSpeed)) / Math.max(maximumRobotSpeed, 1e-6); - c.speedTerm = 1.0 + speedCoefficient * (vNorm * vNorm); - - c.total = calibrationFactor * c.tagScale * c.distanceTerm * c.angleTerm * c.speedTerm; - return c; - } - - private double computeMeasurementNoise( - double baseNoise, - double distanceCoefficient, - double angleCoefficient, - double speedCoefficient, - double distance, - double angleRad, - double robotSpeed, - int tagCount) { - NoiseComponents c = - computeNoiseComponents( - baseNoise, - distanceCoefficient, - angleCoefficient, - speedCoefficient, - distance, - angleRad, - robotSpeed, - tagCount); - return c.total; - } - - /** Helper class to store pose estimation results with confidence metrics. */ - private static class PoseEstimateResult { - public final Pose2d pose; - public final double confidence; - public final double averageDistance; - public final double averageAngle; - public final double currentSpeed; - public final int tagCount; - public final double timestamp; - - public PoseEstimateResult( - Pose2d pose, - double confidence, - double averageDistance, - double averageAngle, - double currentSpeed, - int tagCount, - double timestamp) { - this.pose = pose; - this.confidence = confidence; - this.averageDistance = averageDistance; - this.averageAngle = averageAngle; - this.currentSpeed = currentSpeed; - this.tagCount = tagCount; - this.timestamp = timestamp; - } - } - - /** - * Computes the true 3D off-axis viewing angle (skew) for a tag. Returns angle in radians: 0 = - * head-on, increases with obliqueness. Geometric skew: ACUTE angle between camera +X and tag face - * normal (−Z_tag). - */ - private double computeTargetSkewRad(PhotonTrackedTarget t) { - // Geometric skew: ACUTE angle between camera +X and tag face normal (−Z_tag). - // Use inverse rotation to express tag +Z in the camera frame. - Rotation3d r = t.getBestCameraToTarget().getRotation(); - Translation3d nCam = new Translation3d(0.0, 0.0, 1.0).rotateBy(r.unaryMinus()); - - // Flip so the normal points into the scene, then take the acute angle (abs dot) vs +X. - double nx = -nCam.getX(); - double ny = -nCam.getY(); - double nz = -nCam.getZ(); - - double nNorm = Math.sqrt(nx * nx + ny * ny + nz * nz); - if (nNorm < 1e-9) return 0.0; - - // Absolute dot to make it invariant to 180° normal flips; yields [0, 90°]. - double cosAng = Math.abs(nx) / nNorm; // camera forward is (1,0,0) - cosAng = Math.max(-1.0, Math.min(1.0, cosAng)); - return Math.acos(cosAng); - } - - /** - * Weighted RMS of per-tag skew angles (radians) using geometric skew. Weights favor closer and - * front-on tags: w_i = cos^2(skew_i) / (d_i^2 + eps) - */ - private double computeWeightedRmsSkewRad(List tags) { - double num = 0.0, den = 0.0; - for (var t : tags) { - double s = computeTargetSkewRad(t); // radians from geometry - double d = t.getBestCameraToTarget().getTranslation().getNorm(); - double c = Math.cos(Math.min(s, 0.999 * maximumViewingAngle)); - double w = (c * c) / (d * d + 1e-6); - num += w * s * s; - den += w; - } - if (den <= 1e-9) return 0.0; - return Math.sqrt(num / den); - } -} From 1cf7a3a4e8b51612588e77826556845ef0131a04 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 20 Sep 2025 15:18:05 -0700 Subject: [PATCH 39/43] vision code switching to arjuns laptop (contains errors) --- .../java/frc/robot/subsystems/AnthonyVision.java | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 0f6c7eae..096ccc85 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -46,12 +46,18 @@ public class AnthonyVision extends SubsystemBase { // Noise parameters private double calibrationFactor = 1.0; // constant multiplier to everything - private double baseNoiseX = 0.01; // meters - private double baseNoiseY = 0.01; + private double baseNoiseX = 0.00; // meters + private double baseNoiseY = 0.00; private double baseNoiseTheta = 0.5; // radians - private double distanceCoefficientX = 0.06; - private double distanceCoefficientY = 0.06; + // private double distanceCoefficientX = 0.06; + // private double distanceCoefficientY = 0.06; + + private double distanceExponentialCoefficientX = 0.00046074; + private double distanceExponentialBaseX = 2.97294; + private double distanceExponentialCoefficientY = 0.00046074; + private double distanceExponentialBaseY = 2.97294; + private double distanceCoefficientTheta = 0.9; private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle From 150440f214d7baa90c09ded541c59f0dc1edbc7f Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Mon, 22 Sep 2025 19:26:29 -0700 Subject: [PATCH 40/43] chezy changes --- .../frc/robot/subsystems/AnthonyVision.java | 48 +++++++++++++++---- 1 file changed, 40 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 096ccc85..192a525e 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -46,8 +46,8 @@ public class AnthonyVision extends SubsystemBase { // Noise parameters private double calibrationFactor = 1.0; // constant multiplier to everything - private double baseNoiseX = 0.00; // meters - private double baseNoiseY = 0.00; + private double baseNoiseX = 0.0008; // meters + private double baseNoiseY = 0.0008; private double baseNoiseTheta = 0.5; // radians // private double distanceCoefficientX = 0.06; @@ -210,25 +210,27 @@ public void addFilteredPose() { Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); double nX = - computeNoise( + computeNoiseXY( baseNoiseX, - distanceCoefficientX, + distanceExponentialCoefficientX, + distanceExponentialBaseX, angleCoefficientX, speedCoefficientX, averageDistance, currentSpeed, tagCount); double nY = - computeNoise( + computeNoiseXY( baseNoiseY, - distanceCoefficientY, + distanceExponentialCoefficientY, + distanceExponentialBaseY, angleCoefficientY, speedCoefficientY, averageDistance, currentSpeed, tagCount); double nTH = - computeNoise( + computeNoiseHeading( baseNoiseTheta, distanceCoefficientTheta, angleCoefficientTheta, @@ -283,7 +285,37 @@ private boolean isNotChopped(double yaw) { return (Math.abs(yaw) < 60d); } - private double computeNoise( + private double computeNoiseXY( + double baseNoise, + double distanceExponentialCoefficient, + double distanceExponentialBase, + double angleCoefficient, + double speedCoefficient, + double distance, + double robotSpeed, + int tagCount) { + + // Tag count factor (diminishing returns; cap at 4) + int effectiveTags = Math.min(tagCount, 4); + double tagFactor = 1.0 / Math.sqrt(effectiveTags); + + // Distance term (keep as d^2) + double distanceFactor = baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance); + + // Speed term (quadratic, saturated) + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; + double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); + DogLog.log("Vision/calibrationFactor", calibrationFactor); + DogLog.log("Vision/tagFactor", tagFactor); + DogLog.log("Vision/distanceFactor", distanceFactor); + DogLog.log("Vision/speedFactor", speedFactor); + + double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; + return computedStdDevs; + } + + + private double computeNoiseHeading( double baseNoise, double distanceCoefficient, double angleCoefficient, From 9c84264cb9e6a61438e56ae25c982bdd89eb45f6 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Sat, 27 Sep 2025 00:37:22 -0700 Subject: [PATCH 41/43] gigantic commit --- src/main/java/frc/robot/Robot.java | 2 - src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/commandGroups/JamesHardenScore.java | 25 --------- .../ArmToAngleAndSpinFlywheel.java | 1 - .../frc/robot/subsystems/AnthonyVision.java | 22 +++++--- .../frc/robot/subsystems/ArmSubsystem.java | 12 ++--- .../robot/subsystems/ElevatorSubsystem.java | 54 +++++++++---------- .../frc/robot/subsystems/FunnelSubsystem.java | 10 ++-- .../subsystems/TootsieSlideSubsystem.java | 4 +- 9 files changed, 53 insertions(+), 79 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0754a4e7..9ec8beb7 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -174,8 +174,6 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override public void teleopPeriodic() { - // DogLog.log("PID Constant", IncreasePArm.broomIndex()); - // DogLog.log("Mechanism Type", IncreasePArm.mechIndex()); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b024aee0..89f40b82 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -478,7 +478,7 @@ public Command getAutonomousCommand() { /* Run the path selected from the auto chooser */ int autoValue = autoChooser.getSelected(); Command autoCommand; - DogLog.log("Auto/Selected", autoValue); + DogLog.log("Info/AutoSelected", autoValue); switch (autoValue) { case 1: autoCommand = diff --git a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java index 3541c471..1122e68f 100644 --- a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java +++ b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java @@ -92,30 +92,5 @@ public JamesHardenScore( .andThen(elevateCommand)), new ParallelDeadlineGroup( new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand)); - // addCommands( - // maintainCommand - // .alongWith( - // (new EndWhenCloseEnough(() -> maintainCommand.getTargetPose2d())) - // .andThen(elevateCommand)) - // .alongWith( - // new EndWhenCloseEnough(() -> maintainCommand.getTargetPose2d(), - // Constants.HardenConstants.RegularCommand.xyIndividualTolerance * Math.sqrt(2), - // Constants.HardenConstants.RegularCommand.headingTolerance) - // .andThen(new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5)))); - // ) - // new ParallelDeadlineGroup( - // new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand)); - // } else { - // DogLog.log("JamesHardenScore/Version", "auto"); - // addCommands( - // movementCommand - // .withTimeout(5.0) - // .alongWith( - // (new EndWhenCloseEnough(() -> movementCommand.getTargetPose2d())) - // .andThen(elevateCommand)), - // new ParallelDeadlineGroup( - // new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand)); - // new EndWhenCloseEnough(() -> movementCommand.getTargetPose2d(), 2.8284271247); - // } } } diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java index c0c79071..539c037b 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java @@ -27,7 +27,6 @@ public void initialize() {} @Override public void execute() { armPlusFlywheel.spinFlywheel(); // Spin the flywheel at desired speed - DogLog.log("Commands/ArmToAngleAndSpinFlywheel/TargetAngle", angle); armPlusFlywheel.setPosition(angle); } diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 192a525e..877a8646 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -40,6 +40,7 @@ public class AnthonyVision extends SubsystemBase { // Data type is "Cameras", an enum defined in Constants.java with only two options (left, right) private final Constants.Vision.Cameras cameraId; + private String camTitle; // Reef tag IDs for each side of the field private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); @@ -94,6 +95,8 @@ public AnthonyVision(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSid new PhotonPoseEstimator( fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + camTitle = cameraId.getLoggingName(); + latestVisionResult = null; } @@ -119,9 +122,11 @@ public void periodic() { // If the current camera isn't connected, there's nothing to do here if (!cameraConnected) { + DogLog.log("Vision/" + camTitle + "/CameraConnected", false); return; } + DogLog.log("Vision/" + camTitle + "/CameraConnected", true); // Get all unread results List results = photonCamera.getAllUnreadResults(); @@ -131,19 +136,13 @@ public void periodic() { } } - /** - * Internal method to handle pose estimation with optional confidence checking. - * - * @param useTrigsolve If true, uses trigsolve strategy - * @param forceAdd If true, bypasses confidence checking (used for non-trigsolve or single camera) - * @return PoseEstimateResult containing the pose and confidence data, or null if failed - */ public void addFilteredPose() { PhotonPoseEstimator selectedEstimator = poseEstimator; - String camTitle = cameraId.getLoggingName(); if (latestVisionResult == null || !latestVisionResult.hasTargets()) { + DogLog.log("Vision/" + camTitle + "/HasTargets", false); return; } + DogLog.log("Vision/" + camTitle + "/HasTargets", true); double averageDistance = latestVisionResult.getTargets().stream() @@ -180,8 +179,10 @@ public void addFilteredPose() { DogLog.log("Vision/" + camTitle + "/allTagIds", allTagIds); if (validTags.isEmpty()) { + DogLog.log("Vision/" + camTitle + "/ValidTags", false); return; } + DogLog.log("Vision/" + camTitle + "/ValidTags", true); // Log all tags that haven't been thrown out int tagCount = validTags.size(); @@ -191,20 +192,25 @@ public void addFilteredPose() { // nothing to do if rejected based on the minDistance or if no min dist has been found if (Double.isNaN(minDistance) || minDistance > maximumAllowedDistance) { + DogLog.log("Vision/" + camTitle + "/ThrownOutDistance", true); return; } + DogLog.log("Vision/" + camTitle + "/ThrownOutDistance", false); // find the current speed double currentSpeed = Math.hypot( swerveDrive.getRobotSpeeds().vxMetersPerSecond, swerveDrive.getRobotSpeeds().vyMetersPerSecond); + DogLog.log("Vision/" + camTitle + "/VisionDrivebaseSpeed", currentSpeed); // Get the pose from PhotonVision Optional maybePose = selectedEstimator.update(latestVisionResult); if (maybePose.isEmpty()) { + DogLog.log("Vision/" + camTitle + "/AvailablePose", false); return; } + DogLog.log("Vision/" + camTitle + "/AvailablePose", true); EstimatedRobotPose estimatedPose = maybePose.get(); Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 31289751..366ec512 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -159,7 +159,7 @@ public boolean checkCurrent() { private void runFlywheelAtRPS(double flywheelSpeed) { double motor_speed = flywheelSpeed / Constants.Flywheel.GEAR_RATIO; motor_speed = MathUtil.clamp(motor_speed, -100, 100); - DogLog.log("subsystems/Dale/Target Motor Speed", motor_speed); + DogLog.log("Subsystems/Dale/Target Motor Speed", motor_speed); flywheelMotor.setControl(controlRequestFlywheel.withVelocity(motor_speed)); } @@ -194,15 +194,15 @@ public void periodic() { encoderDegrees = Constants.Arm.ROTATIONS_TO_DEGEREES(armMotor.getPosition(false).getValueAsDouble()); // This method will be called once per scheduler run - DogLog.log("subsystems/Dale/Arm at target", atTarget(5)); - DogLog.log("subsystems/Dale/Arm Degrees", encoderDegrees); + DogLog.log("Subsystems/Dale/Arm at target", atTarget(5)); + DogLog.log("Subsystems/Dale/Arm Degrees", encoderDegrees); DogLog.log( - "subsystems/Dale/Arm Target Rotations", Constants.Arm.DEGREES_TO_ROTATIONS(targetDegrees)); - DogLog.log("subsystems/Dale/Arm Target Degrees", targetDegrees); + "Subsystems/Dale/Arm Target Rotations", Constants.Arm.DEGREES_TO_ROTATIONS(targetDegrees)); + DogLog.log("Subsystems/Dale/Arm Target Degrees", targetDegrees); // DogLog.log( // "subsystems/Dale/Flywheel Speed", flywheelMotor.getVelocity(false).getValueAsDouble()); DogLog.log( - "subsystems/Dale/command", + "Subsystems/Dale/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a00efcb2..a9595664 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -143,7 +143,7 @@ public void resetPositionFiltered() { master.setPosition( currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -153,7 +153,7 @@ public void resetPosition() { this.getToFDistance() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", this.getToFDistance() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -164,7 +164,7 @@ public void resetPosition(double posInHeight) { master.setPosition( posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -194,8 +194,8 @@ public void resetElevatorPositionToZero() { public boolean checkCurrent() { double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); + DogLog.log("Subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); + DogLog.log("Subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); if (Supplycurrent > 1.0 && Statorcurrent > 20) { return true; @@ -232,14 +232,14 @@ public void setPosition(double height) { / ElevatorConstants.CARRAIGE_UPDUCTION) .withSlot(0)); DogLog.log( - "subsystems/Elevator/elevatorSetpoint(rot)", + "Subsystems/Elevator/elevatorSetpoint(rot)", height * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS / ElevatorConstants.CARRAIGE_UPDUCTION); } public void ElevatorTorqueMode() { - DogLog.log("subsystems/Elevator/usingTorqueMode", true); + DogLog.log("Subsystems/Elevator/usingTorqueMode", true); master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); } @@ -261,7 +261,7 @@ public boolean canFunnelTransferCoralToScoring() { public double getToFDistance() { // 0.11 is the sensor offset DogLog.log( - "subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); + "Subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET; } @@ -277,38 +277,38 @@ public void elevatorHasBeenZeroed() { public void periodic() { currentHeightToF = elevatorFilter.calculate(getToFDistance()); // Time of Flight Sensor - DogLog.log("subsystems/Elevator/getError", getError()); - DogLog.log("subsystems/Elevator/ToF/Distance", getToFDistance()); - DogLog.log("subsystems/Elevator/ToF/Connected", distance.isConnected()); - DogLog.log("subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); - - DogLog.log("subsystems/Elevator/isAtPosition", this.isAtPosition()); - DogLog.log("subsystems/Elevator/targetPosition", currentLevel.getPosition()); - DogLog.log("subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); + DogLog.log("Subsystems/Elevator/getError", getError()); + DogLog.log("Subsystems/Elevator/ToF/Distance", getToFDistance()); + DogLog.log("Subsystems/Elevator/ToF/Connected", distance.isConnected()); + DogLog.log("Subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); + + DogLog.log("Subsystems/Elevator/isAtPosition", this.isAtPosition()); + DogLog.log("Subsystems/Elevator/targetPosition", currentLevel.getPosition()); + DogLog.log("Subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); DogLog.log( - "subsystems/Elevator/targetHeightRot", + "Subsystems/Elevator/targetHeightRot", currentLevel.getHeight() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS / Constants.ElevatorConstants.CARRAIGE_UPDUCTION); DogLog.log( - "subsystems/Elevator/currentHeightDist", + "Subsystems/Elevator/currentHeightDist", master.getPosition().getValueAsDouble() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_ROTATIONS_TO_DISTANCE * Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log("subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); + DogLog.log("Subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); DogLog.log( - "subsystems/Elevator/command", + "Subsystems/Elevator/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); DogLog.log( - "subsystems/Elevator/resetPositionBoolean", + "Subsystems/Elevator/resetPositionBoolean", this.isAtPosition() && this.getLevel().equals(ElevatorPositions.Intake)); DogLog.log( - "subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log("subsystems/Elevator/targetLevel", this.getLevel().toString()); + "Subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); + DogLog.log("Subsystems/Elevator/targetLevel", this.getLevel().toString()); DogLog.log( - "subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); + "Subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); DogLog.log( - "subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); + "Subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); } @Override @@ -321,9 +321,5 @@ public void simulationPeriodic() { double newPosition = currentPosition + simulatedSpeed * 0.02; // Assuming a 20ms loop master.setPosition( newPosition); // Alarming to have this since running this on the robot will lead to - - // Log simulation data for debugging - DogLog.log("Simulated Position", newPosition); - DogLog.log("Simulated Speed", simulatedSpeed); } } diff --git a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java index 543bb5c7..798a2fbe 100644 --- a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java @@ -195,13 +195,13 @@ public boolean drakeTripped() { @Override public void periodic() { // This method will be called once per scheduler run - DogLog.log("subsystems/Funnel/CheckedInStatus", isCoralCheckedIn()); - DogLog.log("subsystems/Funnel/CheckedOutStatus", isCoralCheckedOut()); - DogLog.log("subsystems/Funnel/DrakeStatus", drakeTripped()); + DogLog.log("Subsystems/Funnel/CheckedInStatus", isCoralCheckedIn()); + DogLog.log("Subsystems/Funnel/CheckedOutStatus", isCoralCheckedOut()); + DogLog.log("Subsystems/Funnel/DrakeStatus", drakeTripped()); // DogLog.log("subsystems/Funnel/FunnelVelocity", rightMotor.getVelocity().getValueAsDouble()); - DogLog.log("subsystems/Funnel/AbsPositionalError", getAbsolutePositionalError()); + DogLog.log("Subsystems/Funnel/AbsPositionalError", getAbsolutePositionalError()); DogLog.log( - "subsystems/Funnel/command", + "Subsystems/Funnel/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } diff --git a/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java b/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java index fc705c73..ea9e4677 100644 --- a/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java @@ -77,7 +77,7 @@ public static TootsieSlideSubsystem getInstance() { private void runTootsieAtRPS(double flywheelSpeed) { double motor_speed = flywheelSpeed / Constants.TootsieSlide.GEAR_RATIO; - DogLog.log("subsystems/tootsieslide/Target Motor Speed", motor_speed); + DogLog.log("Subsystems/tootsieslide/Target Motor Speed", motor_speed); master.setControl(m_velocity.withVelocity(motor_speed)); m_flywheelSim.setInputVoltage(master.getSupplyVoltage().getValueAsDouble()); @@ -113,7 +113,7 @@ public void periodic() { // DogLog.log("subsystems/tootsieslide/tootsieVelocity", // master.getVelocity().getValueAsDouble()); DogLog.log( - "subsystems/tootsieslide/command", + "Subsystems/tootsieslide/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } From 142615363a68ad841fd9d9dc1231aa38d706bb41 Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Mon, 29 Sep 2025 22:15:24 -0700 Subject: [PATCH 42/43] end of chezy + vision cap change --- src/main/java/frc/robot/Constants.java | 6 +++--- src/main/java/frc/robot/subsystems/AnthonyVision.java | 4 +++- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6e8464ca..5095db4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -926,15 +926,15 @@ public static class ElevatorConstants { public static final double STATOR_CURRENT_LIMIT = 50.0; // TODO: change for actual match public static final double SUPPLY_CURRENT_LIMIT = 30.0; // TODO: change for actual match - public static double S0C_KP = 1.0; // 1.0 before (okay) - public static double S0C_KI = 0.0; + public static double S0C_KP = 1.04; // 1.0 before (okay) + public static double S0C_KI = 0.00; public static double S0C_KD = 0.005; public static double S1C_KP = 0.3501; public static double S1C_KI = 0.0; public static double S1C_KD = 0.0; - public static double S0C_KS = 0.0; + public static double S0C_KS = 0.00; public static double S0C_KG = 0.29 + 0.05; public static double S0C_KA = 0.0004657452997; // 0.04 public static double S0C_KV = 0.124; // 10.66 diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index 877a8646..fdb763eb 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -306,8 +306,10 @@ private double computeNoiseXY( double tagFactor = 1.0 / Math.sqrt(effectiveTags); // Distance term (keep as d^2) - double distanceFactor = baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance); + // double distanceFactor = baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance); + double distanceFactor = (distance < (17.548+0.67)) ? Math.min(baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance), 1.167) : (baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance)); + // Speed term (quadratic, saturated) double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); From b27cbe4cbb851f2b0aa4ba1e62d518db87233d6b Mon Sep 17 00:00:00 2001 From: 73anthonyL <73anthony.lu@gmail.com> Date: Mon, 17 Nov 2025 19:41:46 -0800 Subject: [PATCH 43/43] spotless --- src/main/java/frc/robot/Robot.java | 4 +--- .../ArmToAngleAndSpinFlywheel.java | 1 - .../SwerveCommands/ApplySwerveVoltage.java | 1 - .../SwerveCommands/JamesHardenMovement.java | 22 ++++++++++++++----- .../frc/robot/subsystems/AnthonyVision.java | 21 ++++++++++++------ .../frc/robot/subsystems/SwerveSubsystem.java | 7 ++++-- 6 files changed, 36 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 9ec8beb7..0abcf2c2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -73,7 +73,6 @@ public void robotPeriodic() { visionRight.addFilteredPose(); visionLeft.addFilteredPose(); - DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel()); DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide()); } @@ -173,8 +172,7 @@ public void teleopInit() { /** This function is called periodically during operator control. */ @Override - public void teleopPeriodic() { - } + public void teleopPeriodic() {} @Override public void testInit() { diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java index 539c037b..a3401471 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java +++ b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java @@ -1,7 +1,6 @@ // TODO: THIS COMMAND NEED TO BE UPDATED TO WORK PROPERLY package frc.robot.commands.DaleCommands; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.ElevatorConstants.ElevatorPositions; import frc.robot.subsystems.ArmSubsystem; diff --git a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java index 3e0870ce..a11cdcf5 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java @@ -1,7 +1,6 @@ package frc.robot.commands.SwerveCommands; import com.ctre.phoenix6.controls.MotionMagicVoltage; -import dev.doglog.DogLog; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.SwerveSubsystem; diff --git a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java index 93a12da4..2da6a0dd 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java @@ -102,16 +102,26 @@ public void execute() { DogLog.log("Commands/JamesHarden/TargetPose", targetPose); DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseX(m)", targetPose.getX()); DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseY(m)", targetPose.getY()); - DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseHeading(deg)", targetPose.getRotation().getRadians()); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/TargetPoseHeading(deg)", + targetPose.getRotation().getRadians()); - DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsX(mps)", speeds.vxMetersPerSecond); - DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsY(mps)", speeds.vyMetersPerSecond); - DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsTheta(radps)", speeds.omegaRadiansPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsX(mps)", + speeds.vxMetersPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsY(mps)", + speeds.vyMetersPerSecond); + DogLog.log( + "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsTheta(radps)", + speeds.omegaRadiansPerSecond); DogLog.log( - "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)", swerve.getFieldSpeeds().vxMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)", + swerve.getFieldSpeeds().vxMetersPerSecond); DogLog.log( - "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)", swerve.getFieldSpeeds().vyMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)", + swerve.getFieldSpeeds().vyMetersPerSecond); DogLog.log( "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(radps)", swerve.getFieldSpeeds().omegaRadiansPerSecond); diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java index fdb763eb..60b14e60 100644 --- a/src/main/java/frc/robot/subsystems/AnthonyVision.java +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -40,7 +40,7 @@ public class AnthonyVision extends SubsystemBase { // Data type is "Cameras", an enum defined in Constants.java with only two options (left, right) private final Constants.Vision.Cameras cameraId; - private String camTitle; + private String camTitle; // Reef tag IDs for each side of the field private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); @@ -182,7 +182,7 @@ public void addFilteredPose() { DogLog.log("Vision/" + camTitle + "/ValidTags", false); return; } - DogLog.log("Vision/" + camTitle + "/ValidTags", true); + DogLog.log("Vision/" + camTitle + "/ValidTags", true); // Log all tags that haven't been thrown out int tagCount = validTags.size(); @@ -306,10 +306,18 @@ private double computeNoiseXY( double tagFactor = 1.0 / Math.sqrt(effectiveTags); // Distance term (keep as d^2) - // double distanceFactor = baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance); + // double distanceFactor = baseNoise + + // distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance); + + double distanceFactor = + (distance < (17.548 + 0.67)) + ? Math.min( + baseNoise + + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance), + 1.167) + : (baseNoise + + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance)); - double distanceFactor = (distance < (17.548+0.67)) ? Math.min(baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance), 1.167) : (baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance)); - // Speed term (quadratic, saturated) double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); @@ -317,12 +325,11 @@ private double computeNoiseXY( DogLog.log("Vision/tagFactor", tagFactor); DogLog.log("Vision/distanceFactor", distanceFactor); DogLog.log("Vision/speedFactor", speedFactor); - + double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; return computedStdDevs; } - private double computeNoiseHeading( double baseNoise, double distanceCoefficient, diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 51f1f4ff..25f88f6e 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -97,8 +97,11 @@ public SwerveSubsystem( qProfiledPIDController.setIZone(Constants.HardenConstants.QIZONE); headingProfiledPIDController.setIZone(Constants.HardenConstants.HIZONE); - qProfiledPIDController.setIntegratorRange(Constants.HardenConstants.QIRANGE_LOWER, Constants.HardenConstants.QIRANGE_UPPER); - headingProfiledPIDController.setIntegratorRange(Constants.HardenConstants.HIRANGE_LOWER, Constants.HardenConstants.HIRANGE_UPPER); // 0.3 before + qProfiledPIDController.setIntegratorRange( + Constants.HardenConstants.QIRANGE_LOWER, Constants.HardenConstants.QIRANGE_UPPER); + headingProfiledPIDController.setIntegratorRange( + Constants.HardenConstants.HIRANGE_LOWER, + Constants.HardenConstants.HIRANGE_UPPER); // 0.3 before headingProfiledPIDController.enableContinuousInput(-Math.PI, Math.PI); // configureAutoBuilder();