Photonvision IO

src/main/kotlin/com/team4099/lib/math/GeomUtil.kt

@@ -39,3 +39,11 @@ fun Pose2d.purelyTranslateBy(translation2d: Translation2d): Pose2d {
 fun Pose2d.asTransform2d(): Transform2d {
   return Transform2d(Pose2d(0.meters, 0.meters, 0.radians), this)
 }
+
+/**
+ * Returns pose of whatever the transform represents, in the original frame of the transform. For
+ * example, odomTRobot.asPose2d() would give the pose of the robot in the odometry frame.
+ */
+fun Transform2d.asPose2d(): Pose2d {
+  return Pose2d(this.translation, this.rotation)
+}

src/main/kotlin/com/team4099/lib/vision/TimestampedVisionUpdate.kt

@@ -0,0 +1,15 @@
+package com.team4099.lib.vision
+
+import edu.wpi.first.math.Matrix
+import edu.wpi.first.math.numbers.N1
+import edu.wpi.first.math.numbers.N3
+import org.team4099.lib.geometry.Pose2d
+import org.team4099.lib.units.base.Time
+
+/** Represents a single vision pose with a timestamp and associated standard deviations. */
+data class TimestampedVisionUpdate(
+  val timestamp: Time,
+  val fieldTRobot: Pose2d,
+  val stdDevs: Matrix<N3, N1>,
+  val fromVision: Boolean = false
+)

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)
@@ -89,8 +89,8 @@ object RobotContainer {
     }
 
     superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel)
-    vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
-    limelight.poseSupplier = { drivetrain.odometryPose }
+    vision.setDataInterfaces({ drivetrain.fieldTRobot }, { drivetrain.addVisionData(it) })
+    limelight.poseSupplier = { drivetrain.odomTRobot }
   }
 
   fun mapDefaultCommands() {

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt

@@ -1,7 +1,6 @@
 package com.team4099.robot2023.commands.drivetrain
 
 import com.team4099.lib.logging.LoggedTunableValue
-import com.team4099.lib.pathfollow.Trajectory
 import com.team4099.lib.trajectory.CustomHolonomicDriveController
 import com.team4099.lib.trajectory.CustomTrajectoryGenerator
 import com.team4099.lib.trajectory.Waypoint
@@ -227,9 +226,7 @@ class DrivePathCommand(
         desiredState.curvatureRadPerMeter.radians.sin
 
     var nextDriveState =
-      swerveDriveController.calculate(
-        drivetrain.odometryPose.pose2d, desiredState, desiredRotation
-      )
+      swerveDriveController.calculate(drivetrain.odomTRobot.pose2d, desiredState, desiredRotation)
 
     if (leaveOutYAdjustment) {
       nextDriveState =

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/FollowPathPlannerPathCommand.kt

@@ -53,9 +53,9 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
   private val atReference: Boolean
     get() =
       (
-        (lastSampledPose.translation - drivetrain.odometryPose.translation).magnitude.meters <=
+        (lastSampledPose.translation - drivetrain.odomTRobot.translation).magnitude.meters <=
           translationToleranceAtEnd &&
-          (lastSampledPose.rotation - drivetrain.odometryPose.rotation).absoluteValue <=
+          (lastSampledPose.rotation - drivetrain.odomTRobot.rotation).absoluteValue <=
           thetaToleranceAtEnd
         )
 
@@ -176,7 +176,7 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
     }
 
     val currentSpeeds = drivetrain.targetedChassisSpeeds
-    val poseRotation = drivetrain.odometryPose.rotation.inRotation2ds
+    val poseRotation = drivetrain.odomTRobot.rotation.inRotation2ds
     val generatedTrajectory = path.getTrajectory(currentSpeeds, poseRotation)
 
     // Sampling the trajectory for a state that we're trying to target
@@ -194,13 +194,13 @@ class FollowPathPlannerPathCommand(val drivetrain: Drivetrain, val path: PathPla
     Logger.recordOutput(
       "Pathfollow/thetaSetpoint", stateFromTrajectory.targetHolonomicPose.rotation.degrees
     )
-    Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odometryPose.rotation.inDegrees)
+    Logger.recordOutput("Pathfollow/currentTheta", drivetrain.odomTRobot.rotation.inDegrees)
 
     lastSampledPose = Pose2d(stateFromTrajectory.targetHolonomicPose)
 
     val targetedChassisSpeeds =
       swerveDriveController.calculateRobotRelativeSpeeds(
-        drivetrain.odometryPose, stateFromTrajectory
+        drivetrain.odomTRobot, stateFromTrajectory
       )
 
     // Set closed loop request

src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt

@@ -13,7 +13,7 @@ class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command()
   }
 
   override fun initialize() {
-    drivetrain.odometryPose = AllianceFlipUtil.apply(pose)
+    drivetrain.resetFieldFrameEstimator(AllianceFlipUtil.apply(pose))
     Logger.recordOutput(
       "Drivetrain/lastResetPose", Pose2dWPILIB.struct, AllianceFlipUtil.apply(pose).pose2d
     )

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
+    )
 }