From a38451f314c37a874cec1edef7017a7f22dadc1a Mon Sep 17 00:00:00 2001
From: 2491NoMythic <2491nomythic@gmail.com>
Date: Thu, 7 Nov 2024 16:38:11 -0600
Subject: [PATCH] fixed merged positions

---
 .../java/frc/robot/helpers/MythicalMath.java  | 23 +++++++++++++++++++
 .../robot/subsystems/DrivetrainSubsystem.java |  1 +
 .../java/frc/robot/subsystems/Limelight.java  | 19 +++++++--------
 3 files changed, 34 insertions(+), 9 deletions(-)

diff --git a/src/main/java/frc/robot/helpers/MythicalMath.java b/src/main/java/frc/robot/helpers/MythicalMath.java
index 122ff98..bef6a66 100644
--- a/src/main/java/frc/robot/helpers/MythicalMath.java
+++ b/src/main/java/frc/robot/helpers/MythicalMath.java
@@ -4,6 +4,9 @@
 
 package frc.robot.helpers;
 
+import java.security.PublicKey;
+
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 
 /** Add your docs here. */
@@ -19,5 +22,25 @@ public static double DistanceFromOrigin3d(double XfromOrigin, double YfromOrigin
         double Distance2d = Math.sqrt(Math.pow(YfromOrigin, 2)+Math.pow(XfromOrigin, 2));
         return Math.sqrt(Math.pow(Distance2d, 2)+Math.pow(ZfromOrigin, 2));
     }
+
+    public static Pose2d multiplyOnlyPos(Pose2d pose, Double scalar)
+    {
+        return new Pose2d(pose.getX()*scalar,pose.getY()*scalar,pose.getRotation());
+    }
+
+    public static Pose2d divideOnlyPos(Pose2d pose, Double scalar)
+    {
+        return new Pose2d(pose.getX()/scalar, pose.getY()/scalar,pose.getRotation());
+    }
     
+    /**
+     * 
+     * @param pose1
+     * @param pose2
+     * @return  poses added together, with pose1's rotation
+     */
+    public static Pose2d addOnlyPosTogether(Pose2d pose1, Pose2d pose2)
+    {
+     return new Pose2d(pose1.getX()+pose2.getX(), pose1.getY()+pose2.getY() ,pose1.getRotation());
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
index ed3f0f7..7ee105f 100644
--- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
+++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java
@@ -320,6 +320,7 @@ public void updateOdometryWithVision() {
 				doRejectUpdate = true;
 			}
 			if(!doRejectUpdate) {
+				Logger.recordOutput("Vision/MergesPose", estimate.pose);
 				odometer.addVisionMeasurement(estimate.pose, estimate.timestampSeconds);
 			}
 			RobotState.getInstance().LimelightsUpdated = true;
diff --git a/src/main/java/frc/robot/subsystems/Limelight.java b/src/main/java/frc/robot/subsystems/Limelight.java
index f67b628..c63b1c2 100644
--- a/src/main/java/frc/robot/subsystems/Limelight.java
+++ b/src/main/java/frc/robot/subsystems/Limelight.java
@@ -126,15 +126,16 @@ public PoseEstimate mergedPose(PoseEstimate pose1, PoseEstimate pose2) {
 		double limelight2y = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getY();
 		double limelight2x = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getX();
 		double limelight2z = LimelightHelpers.getTargetPose3d_CameraSpace(APRILTAG_LIMELIGHT3_NAME).getZ();
-    // use those coordinates to find the distance to the closest apriltag for each limelight
-    double distance1 = MythicalMath.DistanceFromOrigin3d(limelight1y, limelight1x, limelight1z);
-    double distance2 = MythicalMath.DistanceFromOrigin3d(limelight2y, limelight2x, limelight2z);
-    //ccombines the two positions in an epic, proportional way
-    double confidenceSource1 = 1/distance1;
-    double confidenceSource2 = 1/distance2;
-    Pose2d scaledPose1 = pose1.pose.times(confidenceSource1);
-    Pose2d scaledPose2 = pose2.pose.times(confidenceSource2);
-    Pose2d newPose = scaledPose1.plus(new Transform2d(scaledPose2.getTranslation(), new Rotation2d())).div(confidenceSource1+confidenceSource2);
+        // use those coordinates to find the distance to the closest apriltag for each limelight
+        double distance1 = MythicalMath.DistanceFromOrigin3d(limelight1x, limelight1y, limelight1z);
+        double distance2 = MythicalMath.DistanceFromOrigin3d(limelight2x, limelight2y, limelight2z);
+        //ccombines the two positions in an epic, proportional way
+        double confidenceSource1 = Math.pow(1/distance1,2);
+        double confidenceSource2 = Math.pow(1/distance2,2);
+        Pose2d scaledPose1 = MythicalMath.multiplyOnlyPos(pose1.pose, confidenceSource1);
+        Pose2d scaledPose2 = MythicalMath.multiplyOnlyPos(pose2.pose, confidenceSource2);
+    Pose2d newPose = MythicalMath.divideOnlyPos((MythicalMath.addOnlyPosTogether(scaledPose1, scaledPose2)), (confidenceSource1+confidenceSource2));
+    //Pose2d newPose = scaledPose1.plus(new Transform2d(scaledPose2.getTranslation(), new Rotation2d())).div(confidenceSource1+confidenceSource2);
     pose1.pose = newPose;
     return pose1;
     }