limelight pose estimation

team4099 · Dec 12, 2023 · 3fa807c · 3fa807c
1 parent 3f76027
commit 3fa807c
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 3 deletions.
diff --git 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
@@ -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)