worked on implementing interpolatedUtil

team581 · Jun 8, 2024 · cba8c80 · cba8c80
1 parent 31cf683
commit cba8c80
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 13 deletions.
diff --git a/src/main/java/frc/robot/localization/InterpolationUtil.java b/src/main/java/frc/robot/localization/InterpolationUtil.java
@@ -5,6 +5,7 @@
 package frc.robot.localization;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import java.util.List;
 
 public class InterpolationUtil {
@@ -24,25 +25,24 @@ public static Pose2d interpolatePose(Pose2d visionInput) {
 
     for (var dataPoint : DATA_POINTS) {
       var distancePoint =
-          dataPoint.measuredPose().getTranslation().getDistance(visionInput.getTranslation());
+          dataPoint.visionPose().getTranslation().getDistance(visionInput.getTranslation());
 
       distanceSum += distancePoint;
     }
-
-    Pose2d weightedSum = new Pose2d();
-
+    double weightedX = 0;
+    double weightedY = 0;
+    Rotation2d weightedRotation = new Rotation2d();
     for (var dataPoint : DATA_POINTS) {
       var distancePoint =
-          dataPoint.measuredPose().getTranslation().getDistance(visionInput.getTranslation());
-
-      var result = dataPoint.visionPose().times(distanceSum - distancePoint);
+          dataPoint.visionPose().getTranslation().getDistance(visionInput.getTranslation());
 
-      weightedSum =
-          new Pose2d(
-              weightedSum.getX() + result.getX(),
-              weightedSum.getY() + result.getY(),
-              weightedSum.getRotation().plus(result.getRotation()));
+      var result =
+          dataPoint.visionPose().minus(dataPoint.measuredPose()).times(distanceSum - distancePoint);
+      weightedX += result.getX();
+      weightedY += result.getY();
+      weightedRotation = weightedRotation.plus(result.getRotation());
     }
+    Pose2d weightedSum = new Pose2d(weightedX, weightedY, weightedRotation);
 
     return weightedSum.div(distanceSum);
   }

diff --git a/src/main/java/frc/robot/localization/LocalizationSubsystem.java b/src/main/java/frc/robot/localization/LocalizationSubsystem.java
@@ -27,6 +27,7 @@
 import frc.robot.util.scheduling.SubsystemPriority;
 import frc.robot.vision.LimelightHelpers;
 import frc.robot.vision.VisionSubsystem;
+import frc.robot.localization.InterpolationUtil;
 
 public class LocalizationSubsystem extends LifecycleSubsystem {
   private static final double SHOOT_WHILE_MOVE_LOOKAHEAD = 0.2;
@@ -62,7 +63,7 @@ public LocalizationSubsystem(SwerveSubsystem swerve, ImuSubsystem imu, VisionSub
             SwerveSubsystem.KINEMATICS,
             imu.getRobotHeading(),
             swerve.getModulePositions().toArray(new SwerveModulePosition[4]),
-            new Pose2d());
+            InterpolationUtil.interpolatePose(new Pose2d()));
     odometry =
         new SwerveDriveOdometry(
             SwerveSubsystem.KINEMATICS,