limelight intial testing

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -25,7 +25,8 @@ import com.team4099.robot2023.subsystems.led.Led
 import com.team4099.robot2023.subsystems.led.LedIO
 import com.team4099.robot2023.subsystems.led.LedIOSim
 import com.team4099.robot2023.subsystems.limelight.LimelightVision
-import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
+import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOReal
+import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOSim
 import com.team4099.robot2023.subsystems.manipulator.Manipulator
 import com.team4099.robot2023.subsystems.manipulator.ManipulatorIONeo
 import com.team4099.robot2023.subsystems.manipulator.ManipulatorIOSim
@@ -73,7 +74,7 @@ object RobotContainer {
           Led(object : LedIO {}),
           GameBoy(GameboyIOServer)
         )
-      limelight = LimelightVision(object : LimelightVisionIO {})
+      limelight = LimelightVision(LimelightVisionIOReal)
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
@@ -91,11 +92,12 @@ object RobotContainer {
           Led(LedIOSim),
           GameBoy(GameboyIOServer)
         )
-      limelight = LimelightVision(object : LimelightVisionIO {})
+      limelight = LimelightVision(LimelightVisionIOSim)
     }
 
     vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
-    limelight.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
+    limelight.setDataInterfaces({ drivetrain.odometryPose }, { it })
+    LimelightVisionIOSim.poseSupplier = { drivetrain.odometryPose }
     drivetrain.elevatorHeightSupplier = Supplier { superstructure.elevatorInputs.elevatorPosition }
     drivetrain.objectiveSupplier = Supplier { superstructure.objective }
     limelight.gamePieceToLookFor = { superstructure.objective }

src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt

@@ -69,8 +69,8 @@ object VisionConstants {
 
     val LL_TRANSFORM =
       Transform3d(
-        Translation3d(1.1438.inches, 10.3966.inches, 12.9284.inches),
-        Rotation3d(8.159.degrees, -90.degrees + 61.610.degrees, -14.1254.degrees)
+        Translation3d(11.6569.inches, -2.999.inches, 37.0768.inches),
+        Rotation3d(0.degrees, 90.0.degrees - 49.678.degrees, 0.degrees)
       )
     const val RES_WIDTH = 320
     const val RES_HEIGHT = 240 // no clue what these numbers should be but usnig these for now

src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt

@@ -31,6 +31,7 @@ import org.team4099.lib.units.base.inMilliseconds
 import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.inRadians
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.derived.tan
@@ -111,7 +112,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
 
       val gamePieceLocationIndex =
         if (FMSData.isBlue) gamePieceToLookFor.invoke().autoStagingLocation
-        else 3 - gamePieceToLookFor.invoke().autoStagingLocation
+        else 3 - gamePieceToLookFor().autoStagingLocation
 
       val gamePieceY =
         FieldConstants.StagingLocations.firstY +
@@ -153,7 +154,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 = gamePiecePose.findClosestPose(*searchList.toTypedArray())
+        val closestPose = searchList[0]
 
         trueGamePieces.add(closestPose)
 
@@ -188,6 +189,17 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
           "LimelightVision/estimatedRobotPoses", *robotPoses.map { it.pose2d }.toTypedArray()
         )
 
+      Logger.getInstance()
+        .recordOutput(
+          "LimelightVision/distanceToGamePieceX", robotPoses.getOrNull(0)?.minus(conePoses.getOrNull(0)?.toPose2d() ?: Pose2d())?.translation?.translation2d?.x ?: 0.0
+        )
+
+      Logger.getInstance()
+        .recordOutput(
+          "LimelightVision/distanceToGamePieceY", robotPoses.getOrNull(0)?.minus(conePoses.getOrNull(0)?.toPose2d() ?: Pose2d())?.translation?.translation2d?.y ?: 0.0
+        )
+
+
       visionConsumer.accept(timestampedVisionUpdates)
     } else if (limelightState == LimelightStates.TELEOP_GAME_PIECE_DETECTION) {
       if (inputs.validReading) {
@@ -213,6 +225,18 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
       }
     }
 
+    Logger.getInstance()
+      .recordOutput(
+        "LimelightVision/RawLimelightReadingsTx",
+        inputs.gamePieceTargets.map { it.tx.inDegrees }.toDoubleArray()
+      )
+
+    Logger.getInstance()
+      .recordOutput(
+        "LimelightVision/RawLimelightReadingsTy",
+        inputs.gamePieceTargets.map { it.ty.inDegrees }.toDoubleArray()
+      )
+
     Logger.getInstance()
       .recordOutput(
         "LimelightVision/robotVisiblePieces",
@@ -230,6 +254,24 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
         "LoggedRobot/Subsystems/LimelightLoopTimeMS",
         (Clock.realTimestamp - startTime).inMilliseconds
       )
+    Logger.getInstance().recordOutput("LimelightVision/LimeLightState", limelightState.name)
+    Logger.getInstance()
+      .recordOutput(
+        "LimelightVision/test",
+        Pose3d(
+          FieldConstants.StagingLocations.positionX,
+          FieldConstants.StagingLocations.translations[0]?.y ?: 0.meters,
+          VisionConstants.Limelight.CONE_HEIGHT / 2,
+          Rotation3d()
+        )
+          .relativeTo(
+            LimelightVisionIOSim.poseSupplier
+              .invoke()
+              .toPose3d()
+              .transformBy(VisionConstants.Limelight.LL_TRANSFORM)
+          )
+          .pose3d
+      )
   }
 
   fun solveTargetPoseFromAngle(
@@ -262,18 +304,18 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
           0.degrees..180.degrees
         ) {
           // we are looking at a red node which is facing towards 180 degrees
-          180.0.degrees
+          0.degrees
         } else {
           // we are looking at a blue node which is facing towards 0 degrees
 
-          0.degrees
+          180.degrees
         }
       )
 
     return currentPose
       .toPose3d()
       .transformBy(VisionConstants.Limelight.LL_TRANSFORM)
-      .transformBy(Transform3d(Translation3d(targetTranslation), targetRotation))
+      .transformBy(Transform3d(Translation3d(targetTranslation), Rotation3d(0.degrees, 0.degrees, 180.0.degrees)))
   }
 
   fun xyDistanceFromTarget(target: LimelightReading, targetHeight: Length): Length {
@@ -320,6 +362,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
 
     val xDistanceFromTargetToCamera =
       (targetHeight - cameraTransform.z) / verticalAngleFromCamera.tan
+
     val yDistanceFromTargetToCamera = xDistanceFromTargetToCamera * horizontalAngleFromCamera.tan
 
     val translationFromTargetToCamera =

src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVisionIOSim.kt

@@ -1,25 +1,47 @@
 package com.team4099.robot2023.subsystems.limelight
 
 import com.team4099.lib.hal.Clock
-import com.team4099.lib.logging.LoggedTunableValue
+import com.team4099.robot2023.config.constants.FieldConstants
+import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.util.LimelightReading
+import com.team4099.robot2023.util.toPose3d
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Rotation3d
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.base.percent
+import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
-import org.team4099.lib.units.derived.inDegrees
 import org.team4099.lib.units.derived.radians
+import java.lang.Math.atan
 
 object LimelightVisionIOSim : LimelightVisionIO {
 
-  private val tunableTx =
-    LoggedTunableValue(
-      "LimelightSim/txDegrees", (8.612154).degrees, Pair({ it.inDegrees }, { it.degrees })
-    )
-  private val tunableTy =
-    LoggedTunableValue(
-      "LimelightSim/tyDegrees",
-      (-1.6576093255536648).degrees,
-      Pair({ it.inDegrees }, { it.degrees })
-    )
+  var poseSupplier: () -> Pose2d = { Pose2d() }
+
+  val poseToGamePiece: Pose3d
+    get() {
+      return Pose3d(
+        FieldConstants.StagingLocations.positionX,
+        FieldConstants.StagingLocations.translations[0]?.y ?: 0.meters,
+        VisionConstants.Limelight.CONE_HEIGHT / 2,
+        Rotation3d(0.0.degrees, 0.0.degrees, 0.0.degrees)
+      )
+        .relativeTo(
+          poseSupplier.invoke().toPose3d().transformBy(VisionConstants.Limelight.LL_TRANSFORM)
+        )
+    }
+
+  private val tunableTx: Angle
+    get() {
+      return atan(poseToGamePiece.y.inMeters / poseToGamePiece.x.inMeters).radians
+    }
+
+  private val tunableTy: Angle
+    get() {
+      return atan(poseToGamePiece.x.inMeters / poseToGamePiece.z.inMeters).radians
+    }
 
   override fun updateInputs(inputs: LimelightVisionIO.LimelightVisionIOInputs) {
     inputs.timestamp = Clock.realTimestamp
@@ -29,10 +51,6 @@ object LimelightVisionIOSim : LimelightVisionIO {
     inputs.fps = 90.0
     inputs.validReading = true
     inputs.gamePieceTargets =
-      listOf(
-        LimelightReading(
-          "", 0.0.percent, tunableTx.get(), tunableTy.get(), 0.0, 0.0, 0.0.percent
-        )
-      )
+      listOf(LimelightReading("cone", 0.0.percent, tunableTx, tunableTy, 0.0, 0.0, 0.0.percent))
   }
 }