remove sim instances

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

@@ -27,10 +27,10 @@ import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
 import com.team4099.robot2023.subsystems.superstructure.Request
 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.subsystems.wrist.Wrist
 import com.team4099.robot2023.subsystems.wrist.WristIO
 import com.team4099.robot2023.subsystems.wrist.WristIOSim
+import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision
 import com.team4099.robot2023.util.driver.Ryan
 import edu.wpi.first.wpilibj.RobotBase
 import org.team4099.lib.smoothDeadband
@@ -59,9 +59,9 @@ object RobotContainer {
         Vision(
           //          object: CameraIO {}
           //          CameraIONorthstar("northstar"),
-          CameraIONorthstar("northstar_1"),
-          CameraIONorthstar("northstar_2"),
-          CameraIONorthstar("northstar_3"),
+          CameraIOPhotonvision("parakeet_1"),
+          CameraIOPhotonvision("parakeet_2"),
+          CameraIOPhotonvision("parakeet_3"),
           //        CameraIONorthstar("right"),
           //        CameraIONorthstar("backward")
         )
@@ -76,9 +76,9 @@ object RobotContainer {
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
       vision =
         Vision(
-          CameraIONorthstar("northstar_1"),
-          CameraIONorthstar("northstar_2"),
-          CameraIONorthstar("northstar_3"),
+          CameraIOPhotonvision("parakeet_1"),
+          CameraIOPhotonvision("parakeet_2"),
+          CameraIOPhotonvision("parakeet_3")
         )
       limelight = LimelightVision(object : LimelightVisionIO {})
       intake = Intake(IntakeIOSim)

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

@@ -1,7 +1,23 @@
 package com.team4099.robot2023.config.constants
 
+import org.team4099.lib.apriltag.AprilTag
+import org.team4099.lib.geometry.Pose3d
 import org.team4099.lib.units.base.feet
+import org.team4099.lib.units.base.inMeters
 
 object FieldConstants {
   val fieldLength = 54.feet
+  val fieldWidth = 26.feet
+
+  val aprilTags: List<AprilTag> = listOf()
+  val homeAprilTags: List<AprilTag> = listOf()
+
+  val wpilibAprilTags =
+    if (Constants.Universal.REAL_FIELD) aprilTags.map { it.apriltagWpilib }
+    else homeAprilTags.map { it.apriltagWpilib }
+
+  val wpilibFieldLayout =
+    edu.wpi.first.apriltag.AprilTagFieldLayout(
+      wpilibAprilTags, fieldLength.inMeters, fieldWidth.inMeters
+    )
 }

src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt

@@ -3,6 +3,7 @@ package com.team4099.robot2023.subsystems.vision
 import com.team4099.lib.hal.Clock
 import com.team4099.lib.logging.TunableNumber
 import com.team4099.lib.vision.TimestampedVisionUpdate
+import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.config.constants.VisionConstants
 import com.team4099.robot2023.subsystems.vision.camera.CameraIO
 import edu.wpi.first.math.VecBuilder
@@ -86,164 +87,61 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
 
     for (instance in io.indices) {
 
-      for (frameIndex in inputs[instance].timestamps.indices) {
-        lastFrameTimes[instance] = Clock.fpgaTime
-        val timestamp = inputs[instance].timestamps[frameIndex]
-        val values = inputs[instance].frames[frameIndex]
-
-        var cameraPose: Pose3d? = null
-        var robotPose: Pose2d? = null
-
-        when (values[0]) {
-          1.0 -> {
-            cameraPose =
-              Pose3d(
-                values[2].meters,
-                values[3].meters,
-                values[4].meters,
-                Rotation3d(Quaternion(values[5].radians, values[6], values[7], values[8]))
-              )
-
-            //
-            // Logger.recordOutput("Vision/${VisionConstants.CAMERA_NAMES[instance]}_transform",
-            // cameraPose.relativeTo(tuningPosition.toPose3d()).pose3d)
-
-            robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d()
-            println(
-              "CameraPoseX: ${cameraPose.x}, transformX: ${cameraPoses[instance].x}, robotPoseX: ${robotPose.x}"
-            )
-          }
-          2.0 -> {
-            val error0 = values[1]
-            val error1 = values[9]
-
-            var use0 = false
-            var use1 = false
-
-            if (error0 < error1 * ambiguityThreshold) {
-              use0 = true
-            } else if (error1 < error0 * ambiguityThreshold) {
-              use1 = true
-            }
-
-            if (use0) {
-              cameraPose =
-                Pose3d(
-                  values[2].meters,
-                  values[3].meters,
-                  values[4].meters,
-                  Rotation3d(Quaternion(values[5].radians, values[6], values[7], values[8]))
-                )
-
-              robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d()
-            } else if (use1) {
-              cameraPose =
-                Pose3d(
-                  values[10].meters,
-                  values[11].meters,
-                  values[12].meters,
-                  Rotation3d(
-                    Quaternion(values[13].radians, values[14], values[15], values[16])
-                  )
-                )
-
-              robotPose = cameraPose.transformBy(cameraPoses[instance].inverse()).toPose2d()
-            } else {
-
-              val cameraPose0 =
-                Pose3d(
-                  values[2].meters,
-                  values[3].meters,
-                  values[4].meters,
-                  Rotation3d(Quaternion(values[5].radians, values[6], values[7], values[8]))
-                )
-
-              val robotPose0 = cameraPose0.transformBy(cameraPoses[instance].inverse()).toPose2d()
-
-              val cameraPose1 =
-                Pose3d(
-                  values[10].meters,
-                  values[11].meters,
-                  values[12].meters,
-                  Rotation3d(
-                    Quaternion(values[13].radians, values[14], values[15], values[16])
-                  )
-                )
-
-              val robotPose1 = cameraPose1.transformBy(cameraPoses[instance].inverse()).toPose2d()
-
-              if (robotPose0.rotation.minus(fieldTCurrentRobotEstimate.rotation).absoluteValue <
-                robotPose1.rotation.minus(fieldTCurrentRobotEstimate.rotation).absoluteValue
-              ) {
-                cameraPose = cameraPose0
-                robotPose = robotPose0
-              } else {
-                cameraPose = cameraPose1
-                robotPose = robotPose1
-              }
-
-              //
-              // Logger.recordOutput("Vision/${VisionConstants.CAMERA_NAMES[instance]}_transform",
-              // cameraPose.relativeTo(tuningPosition.toPose3d()).pose3d)
-            }
-          }
-        }
-
-        if (cameraPose == null || robotPose == null) {
-          continue
-        }
-
-        if ((robotPose.rotation - fieldTCurrentRobotEstimate.rotation).absoluteValue > 7.degrees &&
-          DriverStation.isEnabled()
-        ) {
-          continue
-        }
-
-        // Find all detected tag poses
-        val tagPoses = mutableListOf<Pose3d>()
-        for (i in (if (values[0] == 1.0) 9 else 17) until values.size) {
-          val tagId: Int = values[i].toInt()
-          lastTagDetectionTimes[tagId] = Clock.fpgaTime
-          // TODO: Convert detected tag to the actual pose for 2024
-        }
-
-        // Calculate average distance to tag
-        var totalDistance = 0.0.meters
-        for (tagPose in tagPoses) {
-          totalDistance += tagPose.translation.getDistance(cameraPose.translation)
-        }
-        val averageDistance = totalDistance / tagPoses.size
-
-        // Add to vision updates
-        val xyStdDev = xyStdDevCoefficient.get() * averageDistance.inMeters.pow(2) / tagPoses.size
-        val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size
-
-        visionUpdates.add(
-          TimestampedVisionUpdate(
-            timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev)
-          )
-        )
-        robotPoses.add(robotPose)
+      lastFrameTimes[instance] = Clock.fpgaTime
+      val timestamp = inputs[instance].timestamp
+      val values = inputs[instance].frame
 
-        Logger.recordOutput(
-          "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS",
-          (Clock.fpgaTime - timestamp).inMilliseconds
-        )
+      var cameraPose: Pose3d? = inputs[instance].frame
+      var robotPose: Pose2d? = cameraPose?.transformBy(cameraPoses[instance])?.toPose2d()
 
-        Logger.recordOutput(
-          "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose",
-          Pose2dWPILIB.struct,
-          robotPose.pose2d
-        )
 
-        Logger.recordOutput(
-          "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses",
-          Pose3dWPILIB.struct,
-          *tagPoses.map { it.pose3d }.toTypedArray()
-        )
+      if (cameraPose == null || robotPose == null) {
+        continue
       }
 
-      if (inputs[instance].timestamps.isEmpty()) {
+      if ((robotPose.rotation - currentPose.rotation).absoluteValue > 7.degrees &&
+        DriverStation.isEnabled()
+      ) {
+        continue
+      }
+
+      // Find all detected tag poses
+      val tagPoses = inputs[instance].usedTargets.map { FieldConstants.aprilTags[it].pose }
+
+      // Calculate average distance to tag
+      var totalDistance = 0.0.meters
+      for (tagPose in tagPoses) {
+        totalDistance += tagPose.translation.getDistance(cameraPose.translation)
+      }
+      val averageDistance = totalDistance / tagPoses.size
+
+      // Add to vision updates
+      val xyStdDev = xyStdDevCoefficient.get() * averageDistance.inMeters.pow(2) / tagPoses.size
+      val thetaStdDev = thetaStdDev.get() * averageDistance.inMeters.pow(2) / tagPoses.size
+
+      visionUpdates.add(
+        PoseEstimator.TimestampedVisionUpdate(
+          timestamp, robotPose, VecBuilder.fill(xyStdDev, xyStdDev, thetaStdDev)
+        )
+      )
+      robotPoses.add(robotPose)
+
+      Logger.recordOutput(
+        "Vision/${VisionConstants.CAMERA_NAMES[instance]}/latencyMS",
+        (Clock.fpgaTime - timestamp).inMilliseconds
+      )
+
+      Logger.recordOutput(
+        "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose", robotPose.pose2d
+      )
+
+      Logger.recordOutput(
+        "Vision/${VisionConstants.CAMERA_NAMES[instance]}/tagPoses",
+        *tagPoses.map { it.pose3d }.toTypedArray()
+      )
+
+
+      if (inputs[instance].timestamp == 0.0.seconds) { // prolly wrong lol
         Logger.recordOutput(
           "Vision/${VisionConstants.CAMERA_NAMES[instance]}/estimatedRobotPose",
           Pose2dWPILIB.struct,

src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIO.kt

@@ -2,39 +2,34 @@ package com.team4099.robot2023.subsystems.vision.camera
 
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
-import org.team4099.lib.units.base.Time
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Pose3dWPILIB
 import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.base.seconds
 
 interface CameraIO {
   class CameraInputs : LoggableInputs {
-    var timestamps = listOf<Time>()
-    var frames = listOf<DoubleArray>()
+    var timestamp = 0.0.seconds
+    var frame: Pose3d = Pose3d()
     var fps = 0.0
+    var usedTargets: List<Int> = listOf<Int>()
 
     override fun toLog(table: LogTable?) {
-      table?.put("timestampsSeconds", timestamps.map { it.inSeconds }.toDoubleArray())
-      table?.put("frameCount", frames.size.toDouble())
-      for (i in frames.indices) {
-        table?.put("Frame/$i", frames[i])
-      }
+      table?.put("timestampSeconds", timestamp.inSeconds)
+      table?.put("frame", frame.pose3d)
       table?.put("fps", fps)
+      table?.put("usedTargets", usedTargets)
     }
 
     override fun fromLog(table: LogTable?) {
-      table?.get("timestampsSeconds", timestamps.map { it.inSeconds }.toDoubleArray())?.let {
-        returnedTimestamps ->
-        timestamps = returnedTimestamps.map { it.seconds }
+      table?.get("timestampSeconds", 0.0)?.let {
+        timestamp = it.seconds
       }
-
-      val frameCount = table?.get("frameCount", 0.0)?.toInt() ?: 0
-      val tempFrames = mutableListOf<DoubleArray>()
-      for (i in 0 until frameCount) {
-        tempFrames.add(table?.get("Frame/$i", DoubleArray(0)) ?: DoubleArray(0))
+      table?.get("frame", Pose3dWPILIB())?.let {
+        frame = Pose3d(it)
       }
-      frames = tempFrames.toList()
-
-      table?.get("fps", fps)?.let { fps = it }
+      table?.get("fps", 0.0)
+      table?.get("usedTargets", listOf<Double>())
     }
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/vision/camera/CameraIOPhotonvision.kt

@@ -1,4 +1,55 @@
 package com.team4099.robot2023.subsystems.vision.camera
 
-class CameraIOPhotonvision {
+import com.team4099.robot2023.config.constants.FieldConstants
+import org.photonvision.EstimatedRobotPose
+import org.photonvision.PhotonCamera
+import org.photonvision.PhotonPoseEstimator
+import org.photonvision.PhotonPoseEstimator.PoseStrategy
+import org.team4099.lib.around
+import org.team4099.lib.geometry.Pose3d
+import org.team4099.lib.geometry.Pose3dWPILIB
+import org.team4099.lib.geometry.Transform3dWPILIB
+import org.team4099.lib.units.base.Time
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.micro
+import org.team4099.lib.units.milli
+import java.util.*
+
+
+class CameraIOPhotonvision(private val identifier: String): CameraIO {
+
+    private val photonEstimator: PhotonPoseEstimator
+    private val camera: PhotonCamera
+    private var lastEstTimestamp: Time = 0.0.seconds
+
+    init {
+        camera = PhotonCamera(identifier)
+        photonEstimator = PhotonPoseEstimator(
+            FieldConstants.wpilibFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera, Transform3dWPILIB()
+        )
+        photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.LOWEST_AMBIGUITY)
+
+    }
+
+    override fun updateInputs(inputs: CameraIO.CameraInputs) {
+        val visionEst: Optional<EstimatedRobotPose>? = photonEstimator.update()
+        val poseEst = visionEst?.get()?.let { Pose3d(it.estimatedPose) }
+        val latestTimestamp = visionEst?.get()?.timestampSeconds?.seconds
+        if (latestTimestamp != null) {
+            if ((latestTimestamp - lastEstTimestamp).absoluteValue > 10.micro.seconds) {
+                inputs.fps = 1/(latestTimestamp - lastEstTimestamp).inSeconds
+                lastEstTimestamp = latestTimestamp
+            }
+        }
+
+        if (visionEst != null) {
+            inputs.usedTargets = visionEst.get().targetsUsed.map { it.fiducialId }
+        }
+
+        inputs.timestamp = lastEstTimestamp
+        if (poseEst != null) {
+            inputs.frame = poseEst
+        }
+    }
 }