updated the hub position aiming

src/main/java/frc/robot/Landmarks.java

@@ -35,9 +35,11 @@ public static Alliance getAlliance() {
         return cachedAlliance != null ? cachedAlliance : Alliance.Blue;
     }
 
-    public static Translation2d hubPosition() {
+    public static Translation2d hubPosition(Translation2d robotPosition) {
         final Alliance alliance = getAlliance();
-        return alliance == Alliance.Blue ? computeHubPosition(kBlueHubTagIds, kDefaultBlueHubPosition) : computeHubPosition(kRedHubTagIds, kDefaultRedHubPosition);
+        return alliance == Alliance.Blue
+            ? computeHubPosition(kBlueHubTagIds, kDefaultBlueHubPosition, robotPosition)
+            : computeHubPosition(kRedHubTagIds, kDefaultRedHubPosition, robotPosition);
     }
 
     public static final AprilTagFieldLayout layout =
@@ -54,7 +56,6 @@ public static Translation2d hubPosition() {
     public static final Translation2d redDSOffset = redOutpostCenter.plus(new Translation2d(0,fieldWidth - 0.5));
 
     //red hub (backside)
-    public static final Translation2d redHub = computeHubPosition(kRedHubTagIds, kDefaultRedHubPosition);
     public static final Translation2d redHubOffset = getTagPosition(4);
 
     //blue outpost
@@ -65,7 +66,6 @@ public static Translation2d hubPosition() {
     public static final Translation2d blueDSOffset = blueOutpostCenter.minus(new Translation2d(0, fieldWidth -0.5));
 
     //blue hub (backside)
-    public static final Translation2d blueHub = computeHubPosition(kBlueHubTagIds, kDefaultBlueHubPosition);
     public static final Translation2d blueHubOffset = getTagPosition(26);
 
     private static Translation2d computeHubPosition(int[] tagIds, Translation2d fallback) {
@@ -92,23 +92,21 @@ public static Pose2d climbPose() {
         return alliance == Alliance.Blue ? Constants.ClimbAlignment.kBlueAllianceTargetPose : Constants.ClimbAlignment.kRedAllianceTargetPose;
     }
 
-    public static Translation2d getClosestTag(Translation2d robotPosition) {
-        final int[] hubTagIds = getAlliance() == Alliance.Blue ? kBlueHubTagIds : kRedHubTagIds;
-        Translation2d closest = null;
-        double closestDistance = Double.MAX_VALUE;
+    private static Translation2d computeHubPosition(int[] tagIds, Translation2d fallback, Translation2d robotPosition) {
+        Translation2d sum = new Translation2d();
+        double totalWeight = 0;
 
-        for (int id : hubTagIds) {
-            Optional<Pose3d> tagPose = layout.getTagPose(id);
-            if (tagPose.isPresent()) {
-                Translation2d tagPosition = tagPose.get().getTranslation().toTranslation2d();
+        for (int id : tagIds) {
+            Optional<Pose3d> pose = layout.getTagPose(id);
+            if (pose.isPresent()) {
+                Translation2d tagPosition = pose.get().getTranslation().toTranslation2d();
                 double distance = robotPosition.getDistance(tagPosition);
-                if (distance < closestDistance) {
-                    closestDistance = distance;
-                    closest = tagPosition;
-                }
+                double weight = 1.0 / (distance * distance); //can tune this so it works better :)
+                sum = sum.plus(tagPosition.times(weight));
+                totalWeight += weight;
             }
         }
 
-        return closest != null ? closest : hubPosition();
+        return totalWeight > 0 ? sum.div(totalWeight) : fallback;
     }
 }

src/main/java/frc/robot/commands/AimAndDriveCommand.java

@@ -72,7 +72,7 @@ private Rotation2d getTargetHeadingInOperatorPerspective() {
 
     private Rotation2d getTargetHeadingInFieldFrame() {
         final Translation2d robotPosition = swerve.getState().Pose.getTranslation();
-        final Translation2d hubPosition = Landmarks.hubPosition();
+        final Translation2d hubPosition = Landmarks.hubPosition(robotPosition);
         return hubPosition.minus(robotPosition).getAngle();
     }
 
@@ -114,7 +114,8 @@ public void execute() {
         if (now - lastDebugPrintTimestamp >= kDebugPrintIntervalSeconds) {
             lastDebugPrintTimestamp = now;
             final Pose2d pose = swerve.getState().Pose;
-            final Translation2d hubPosition = Landmarks.hubPosition();
+            final Translation2d robotPosition = pose.getTranslation();
+            final Translation2d hubPosition = Landmarks.hubPosition(robotPosition);
             final Rotation2d currentHeading = pose.getRotation();
             final Rotation2d targetFieldHeading = getTargetHeadingInFieldFrame();
             final Rotation2d targetOpHeading = getTargetHeadingInOperatorPerspective();

src/main/java/frc/robot/commands/PrepareShotCommand.java

@@ -66,7 +66,7 @@ public boolean isReadyToShoot() {
 
     private Distance getDistanceToHub() {
         final Translation2d robotPosition = robotPoseSupplier.get().getTranslation();
-        final Translation2d hubPosition = Landmarks.hubPosition();
+        final Translation2d hubPosition = Landmarks.hubPosition(robotPosition);
         return Meters.of(robotPosition.getDistance(hubPosition));
     }