cleaned up methods

CurtinFRC · Jan 24, 2025 · 5af0da6 · 5af0da6
1 parent 8192b81
commit 5af0da6
Showing 1 changed file with 38 additions and 76 deletions.
diff --git a/src/main/java/org/curtinfrc/frc2025/Constants.java b/src/main/java/org/curtinfrc/frc2025/Constants.java
@@ -104,87 +104,49 @@ public Pose3d toPose(Pose3d currentPose) {
 
     private Pose3d resolvePose(List<Integer> tagIds, Pose3d currentPose) {
       if (tagIds.isEmpty()) return new Pose3d();
-
-      double sideOffset = Math.max(ROBOT_X / 1000.0, ROBOT_Y / 1000.0) / 2.0;
-
-      final class Closest {
-        Pose3d pose = null;
-        double distance = Double.MAX_VALUE;
+  
+      double sideOffset = Math.max(ROBOT_X, ROBOT_Y) / 2000.0;
+  
+      class ClosestPose {
+          Pose3d pose = null;
+          double distance = Double.MAX_VALUE;
       }
-
-      Closest closest = new Closest();
-
+  
+      ClosestPose closest = new ClosestPose();
+  
       for (int tagId : tagIds) {
-        aprilTagLayout
-            .getTagPose(tagId)
-            .ifPresent(
-                tagPose -> {
-                  Pose3d mappedPose = PoseUtil.mapPose(tagPose);
-                  Rotation3d rotation = mappedPose.getRotation();
-
-                  double baseAngle = /*normalizeAngle(*/rotation.getAngle()/*)*/;
-
-                  double cosHeading = Math.cos(baseAngle);
-                  double sinHeading = Math.sin(baseAngle);
-
-                  double adjustedXLeft, adjustedYLeft, adjustedXRight, adjustedYRight;
-
-                  adjustedXLeft = mappedPose.getX() + sideOffset * sinHeading;
-                  adjustedYLeft = mappedPose.getY() - sideOffset * cosHeading;
-
-                  adjustedXRight = mappedPose.getX() - sideOffset * sinHeading;
-                  adjustedYRight = mappedPose.getY() + sideOffset * cosHeading;
-
-                  Pose3d leftPose =
-                      new Pose3d(adjustedXLeft, adjustedYLeft, mappedPose.getZ(), rotation);
-
-                  Pose3d rightPose =
-                    new Pose3d(adjustedXRight, adjustedYRight, mappedPose.getZ(), rotation);
-
-                  double leftDistance = distanceBetween(leftPose, currentPose);
-                  if (leftDistance < closest.distance) {
-                    closest.pose = leftPose;
-                    closest.distance = leftDistance;
-                  }
-
-                  double rightDistance = distanceBetween(rightPose, currentPose);
-                  if (rightDistance < closest.distance) {
-                  closest.pose = rightPose;
-                  closest.distance = rightDistance;
+          aprilTagLayout.getTagPose(tagId).ifPresent(tagPose -> {
+              Pose3d mappedPose = PoseUtil.mapPose(tagPose);
+              Rotation3d rotation = mappedPose.getRotation();
+              double baseAngle = rotation.getAngle(), cos = Math.cos(baseAngle), sin = Math.sin(baseAngle);
+              double xOffset = sideOffset * sin, yOffset = sideOffset * cos;
+
+              for (Pose3d pose : new Pose3d[] {
+                  new Pose3d(mappedPose.getX() + xOffset, mappedPose.getY() - yOffset, mappedPose.getZ(), rotation),
+                  new Pose3d(mappedPose.getX() - xOffset, mappedPose.getY() + yOffset, mappedPose.getZ(), rotation)
+              }) {
+                  double distance = distanceBetween(pose, currentPose);
+                  if (distance < closest.distance) {
+                      closest.pose = pose;
+                      closest.distance = distance;
                   }
-
-                  String tagPrefix = String.format("resolvePose.tagId_%d", tagId);
-                  Logger.recordOutput(tagPrefix + "/baseAngle", String.format("%.2f", baseAngle));
-                  Logger.recordOutput(
-                      tagPrefix + "/leftPose",
-                      String.format(
-                          "(%.2f, %.2f, %.2f)", adjustedXLeft, adjustedYLeft, mappedPose.getZ()));
-                  Logger.recordOutput(
-                      tagPrefix + "/rightPose",
-                      String.format(
-                          "(%.2f, %.2f, %.2f)", adjustedXRight, adjustedYRight, mappedPose.getZ()));
-                  Logger.recordOutput(
-                      tagPrefix + "/currentPose",
-                      String.format(
-                          "(%.2f, %.2f, %.2f)",
-                          currentPose.getX(), currentPose.getY(), currentPose.getZ()));
-                  Logger.recordOutput(
-                            tagPrefix + "/tagPose",
-                            String.format(
-                                "(%.2f, %.2f, %.2f)",
-                                tagPose.getX(), tagPose.getY(), tagPose.getZ()));
-                      });
+              }
+
+              String tagPrefix = String.format("resolvePose.tagId_%d", tagId);
+              Logger.recordOutput(tagPrefix + "/baseAngle", String.format("%.2f", baseAngle));
+              Logger.recordOutput(tagPrefix + "/leftPose", formatPose(new Pose3d(mappedPose.getX() + xOffset, mappedPose.getY() - yOffset, mappedPose.getZ(), rotation)));
+              Logger.recordOutput(tagPrefix + "/rightPose", formatPose(new Pose3d(mappedPose.getX() - xOffset, mappedPose.getY() + yOffset, mappedPose.getZ(), rotation)));
+              Logger.recordOutput(tagPrefix + "/currentPose", formatPose(currentPose));
+              Logger.recordOutput(tagPrefix + "/tagPose", formatPose(tagPose));
+          });
       }
-
       return closest.pose != null ? closest.pose : new Pose3d();
-    }
-
-    private double normalizeAngle(double angle) {
-      while (angle > Math.PI) angle -= 2 * Math.PI;
-      while (angle < -Math.PI) angle += 2 * Math.PI;
-      return angle;
-    }
-
+  }
+
+  private String formatPose(Pose3d pose) {
+      return String.format("(%.2f, %.2f, %.2f)", pose.getX(), pose.getY(), pose.getZ());
+  }
+
     private double distanceBetween(Pose3d pose1, Pose3d pose2) {
       var t1 = pose1.getTranslation();
       var t2 = pose2.getTranslation();