tune interpolationUtil

team581 · May 22, 2024 · 31cf683 · 31cf683
1 parent 23ae571
commit 31cf683
Showing 1 changed file with 22 additions and 3 deletions.
diff --git a/src/main/java/frc/robot/localization/InterpolationUtil.java b/src/main/java/frc/robot/localization/InterpolationUtil.java
@@ -20,11 +20,30 @@ public class InterpolationUtil {
       List.of(SUBWOOFER, STAGE_FRONT, STAGE_RIGHT, STAGE_MIDDLE);
 
   public static Pose2d interpolatePose(Pose2d visionInput) {
+    double distanceSum = 0;
+
+    for (var dataPoint : DATA_POINTS) {
+      var distancePoint =
+          dataPoint.measuredPose().getTranslation().getDistance(visionInput.getTranslation());
+
+      distanceSum += distancePoint;
+    }
+
+    Pose2d weightedSum = new Pose2d();
+
     for (var dataPoint : DATA_POINTS) {
-      double distancePoint =
-          dataPoint.visionPose().getTranslation().getDistance(visionInput.getTranslation());
+      var distancePoint =
+          dataPoint.measuredPose().getTranslation().getDistance(visionInput.getTranslation());
+
+      var result = dataPoint.visionPose().times(distanceSum - distancePoint);
+
+      weightedSum =
+          new Pose2d(
+              weightedSum.getX() + result.getX(),
+              weightedSum.getY() + result.getY(),
+              weightedSum.getRotation().plus(result.getRotation()));
     }
 
-    return new Pose2d();
+    return weightedSum.div(distanceSum);
   }
 }