return comments to correct indentation

diff --git a/src/main/java/frc/robot/util/Vision/Vision.java b/src/main/java/frc/robot/util/Vision/Vision.java
index 7e733cef0d61d220e4eb9554b46c49a03a9410f1..71c39f84c0d03ba0353f48c7e6eb7a8d936685e2 100644 (file)
--- a/src/main/java/frc/robot/util/Vision/Vision.java
+++ b/src/main/java/frc/robot/util/Vision/Vision.java
@@ -613,75 +613,71 @@ public class Vision {
       // Set<Short> tagIds = new HashSet<>();
       // List<PoseObservation> poseObservations = new LinkedList<>();
       // for (var result : camera.getAllUnreadResults()) {
-      // // Update latest target observation
-      // if (result.hasTargets()) {
-      // inputs.latestTargetObservation =
-      // new TargetObservation(
-      // Rotation2d.fromDegrees(result.getBestTarget().getYaw()),
-      // Rotation2d.fromDegrees(result.getBestTarget().getPitch()));
-      // } else {
-      // inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new
-      // Rotation2d());
-      // }
-      // }
-      // // Add pose observation
-      // if (result.multitagResult.isPresent()) { // Multitag result
-      // var multitagResult = result.multitagResult.get();
-
-      // // Calculate robot pose
-      // Transform3d fieldToCamera = multitagResult.estimatedPose.best;
-      // Transform3d fieldToRobot =
-      // fieldToCamera.plus(VisionConstants.APRIL_TAG_CAMERAS.get(0).getSecond().inverse());
-      // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(),
-      // fieldToRobot.getRotation());
-
-      // // Calculate average tag distance
-      // double totalTagDistance = 0.0;
-      // for (var target : result.targets) {
-      // totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm();
-      // }
-
-      // // Add tag IDs
-      // tagIds.addAll(multitagResult.fiducialIDsUsed);
-
-      // // Add observation
-      // poseObservations.add(
-      // new PoseObservation(
-      // result.getTimestampSeconds(), // Timestamp
-      // robotPose, // 3D pose estimate
-      // multitagResult.estimatedPose.ambiguity, // Ambiguity
-      // multitagResult.fiducialIDsUsed.size(), // Tag count
-      // totalTagDistance / result.targets.size(), // Average tag distance
-      // PoseObservationType.PHOTONVISION)); // Observation type
-
-      // } else if (!result.targets.isEmpty()) { // Single tag result
-      // var target = result.targets.get(0);
-
-      // // Calculate robot pose
-      // var tagPose = aprilTagLayout.getTagPose(target.fiducialId);
-      // if (tagPose.isPresent()) {
-      // Transform3d fieldToTarget =
-      // new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
-      // Transform3d cameraToTarget = target.bestCameraToTarget;
-      // Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse());
-      // Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse());
-      // Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(),
-      // fieldToRobot.getRotation());
-
-      // // Add tag ID
-      // tagIds.add((short) target.fiducialId);
-
-      // // Add observation
-      // poseObservations.add(
-      // new PoseObservation(
-      // result.getTimestampSeconds(), // Timestamp
-      // robotPose, // 3D pose estimate
-      // target.poseAmbiguity, // Ambiguity
-      // 1, // Tag count
-      // cameraToTarget.getTranslation().getNorm(), // Average tag distance
-      // PoseObservationType.PHOTONVISION)); // Observation type
-      // }
+      //   // Update latest target observation
+      //   if (result.hasTargets()) {
+      //     inputs.latestTargetObservation =
+      //         new TargetObservation(
+      //             Rotation2d.fromDegrees(result.getBestTarget().getYaw()),
+      //             Rotation2d.fromDegrees(result.getBestTarget().getPitch()));
+      //   } else {
+      //     inputs.latestTargetObservation = new TargetObservation(new Rotation2d(), new Rotation2d());
+      //   }
       // }
+      //   // Add pose observation
+      //   if (result.multitagResult.isPresent()) { // Multitag result
+      //     var multitagResult = result.multitagResult.get();
+
+      //     // Calculate robot pose
+      //     Transform3d fieldToCamera = multitagResult.estimatedPose.best;
+      //     Transform3d fieldToRobot = fieldToCamera.plus(VisionConstants.APRIL_TAG_CAMERAS.get(0).getSecond().inverse());
+      //     Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation());
+
+      //     // Calculate average tag distance
+      //     double totalTagDistance = 0.0;
+      //     for (var target : result.targets) {
+      //       totalTagDistance += target.bestCameraToTarget.getTranslation().getNorm();
+      //     }
+
+      //     // Add tag IDs
+      //     tagIds.addAll(multitagResult.fiducialIDsUsed);
+
+      //     // Add observation
+      //     poseObservations.add(
+      //         new PoseObservation(
+      //             result.getTimestampSeconds(), // Timestamp
+      //             robotPose, // 3D pose estimate
+      //             multitagResult.estimatedPose.ambiguity, // Ambiguity
+      //             multitagResult.fiducialIDsUsed.size(), // Tag count
+      //             totalTagDistance / result.targets.size(), // Average tag distance
+      //             PoseObservationType.PHOTONVISION)); // Observation type
+
+      //   } else if (!result.targets.isEmpty()) { // Single tag result
+      //     var target = result.targets.get(0);
+
+      //     // Calculate robot pose
+      //     var tagPose = aprilTagLayout.getTagPose(target.fiducialId);
+      //     if (tagPose.isPresent()) {
+      //       Transform3d fieldToTarget =
+      //           new Transform3d(tagPose.get().getTranslation(), tagPose.get().getRotation());
+      //       Transform3d cameraToTarget = target.bestCameraToTarget;
+      //       Transform3d fieldToCamera = fieldToTarget.plus(cameraToTarget.inverse());
+      //       Transform3d fieldToRobot = fieldToCamera.plus(robotToCamera.inverse());
+      //       Pose3d robotPose = new Pose3d(fieldToRobot.getTranslation(), fieldToRobot.getRotation());
+
+      //       // Add tag ID
+      //       tagIds.add((short) target.fiducialId);
+
+      //       // Add observation
+      //       poseObservations.add(
+      //           new PoseObservation(
+      //               result.getTimestampSeconds(), // Timestamp
+      //               robotPose, // 3D pose estimate
+      //               target.poseAmbiguity, // Ambiguity
+      //               1, // Tag count
+      //               cameraToTarget.getTranslation().getNorm(), // Average tag distance
+      //               PoseObservationType.PHOTONVISION)); // Observation type
+      //     }
+      //   }
       // }
     }