From 3fa807cfe5bda8abc50de355024a8b477566abb6 Mon Sep 17 00:00:00 2001
From: Matthew Choulas <MatthewChoulas@gmail.com>
Date: Mon, 11 Dec 2023 20:13:09 -0500
Subject: [PATCH] limelight pose estimation

---
 src/main/kotlin/com/team4099/robot2023/RobotContainer.kt   | 7 ++++++-
 .../robot2023/subsystems/limelight/LimelightVision.kt      | 5 +++--
 2 files changed, 9 insertions(+), 3 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index 0d8dcafc..687d3743 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -35,9 +35,14 @@ import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar
 import com.team4099.robot2023.util.driver.Ryan
+import edu.wpi.first.math.geometry.Rotation2d
 import edu.wpi.first.wpilibj.RobotBase
 import org.littletonrobotics.junction.Logger
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Transform2d
+import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.smoothDeadband
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import java.util.function.Supplier
@@ -96,7 +101,7 @@ object RobotContainer {
     }
 
     vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
-    limelight.setDataInterfaces({ drivetrain.odometryPose }, { it })
+    limelight.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
     LimelightVisionIOSim.poseSupplier = { drivetrain.odometryPose }
     drivetrain.elevatorHeightSupplier = Supplier { superstructure.elevatorInputs.elevatorPosition }
     drivetrain.objectiveSupplier = Supplier { superstructure.objective }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
index 927f8b88..52fe36c6 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
@@ -7,6 +7,7 @@ import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.subsystems.gameboy.objective.Objective
+import com.team4099.robot2023.util.AllianceFlipUtil
 import com.team4099.robot2023.util.FMSData
 import com.team4099.robot2023.util.LimelightReading
 import com.team4099.robot2023.util.PoseEstimator
@@ -28,6 +29,7 @@ import org.team4099.lib.geometry.Translation3dWPILIB
 import org.team4099.lib.units.base.Length
 import org.team4099.lib.units.base.inMeters
 import org.team4099.lib.units.base.inMilliseconds
+import org.team4099.lib.units.base.inches
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
@@ -66,7 +68,6 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
     AUTO_POSE_ESTIMATION,
     TELEOP_GAME_PIECE_DETECTION
   }
-
   init {
     for (locationIndex in listOf(0, 1, 2, 3)) {
       conePoses.add(
@@ -154,7 +155,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
       for ((index, gamePiecePose) in visibleGamePieces.withIndex()) {
         val searchList =
           if (inputs.gamePieceTargets[index].className == "cone") conePoses else cubePoses
-        val closestPose = searchList[0]
+        val closestPose = gamePiecePose.findClosestPose(*searchList.toTypedArray())
 
         trueGamePieces.add(closestPose)