commiting untested vision code

src/main/java/frc/robot/RobotContainer.java

@@ -125,8 +125,10 @@ private void configureButtonBindings() {
     m_drive.setDefaultCommand(
         DriveCommandFactory.joystickDrive(
             m_drive,
-            () -> -controller.getX(),
+            // () -> -controller.getX(),
+            // () -> -controller.getY(),
             () -> -controller.getY(),
+            () -> -controller.getX(),
             () -> -controller.getZ(),
             0.1));
     // controller.cross().onTrue(Commands.runOnce(m_drive::stopWithX, m_drive));

src/main/java/frc/robot/constants/Constants.java

@@ -1,10 +1,20 @@
 package frc.robot.constants;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
+import org.ejml.simple.SimpleMatrix;
 
 public final class Constants {
   private static RobotType kRobotType = RobotType.ROBOT_SIMBOT;
@@ -25,14 +35,14 @@ public enum RobotType {
   public static RobotType getRobotType() {
     if (RobotBase.isReal() && kRobotType == RobotType.ROBOT_SIMBOT) {
       DriverStation.reportError(
-          "Robot is set to SIM but it isn't a SIM, setting it to Competition Robot as redundancy.",
+          "Robot is set to SIM but isn't a SIM. Setting Robot to Competition Robot as redundancy.",
           false);
       kRobotType = RobotType.ROBOT_2023_OFFSEASON_SWERVE;
     }
 
     if (RobotBase.isSimulation() && kRobotType != RobotType.ROBOT_SIMBOT) {
       DriverStation.reportError(
-          "Robot is set to REAL but it is a SIM, setting it to SIMBOT as redundancy.", false);
+          "Robot is set to REAL but is a SIM. setting Robot to SIMBOT as redundancy.", false);
       kRobotType = RobotType.ROBOT_SIMBOT;
     }
 
@@ -100,4 +110,11 @@ public static Translation2d[] getModuleTranslations() {
     public static final SwerveDriveKinematics m_kinematics =
         new SwerveDriveKinematics(getModuleTranslations());
   }
+
+  public static class Vision {
+    public static final double cameraHeightMeters = 0; //TODO
+    public static final double targetHeightMeters = 0; //TODO
+    public static final double cameraPitchRadians = 0; //TODO
+    public static final double targetPitchRadians = 0; //TODO
+  }
 }

src/main/java/frc/robot/subsystems/drive/DriveBase.java

@@ -12,6 +12,7 @@
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
+
 public class DriveBase extends SubsystemBase {
   public static final double ODOMETRY_FREQUENCY = 250.0;
 
@@ -91,6 +92,8 @@ public void periodic() {
     Logger.recordOutput("Odometry/EstimatedPose", PoseEstimator.getInstance().getPose());
   }
 
+  // public void addVisionMeasurement()
+
   /**
    * Runs the drive at the desired velocity.
    *

src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -0,0 +1,33 @@
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.geometry.Transform3d;
+import java.util.Optional;
+import org.littletonrobotics.junction.AutoLog;
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+
+public interface VisionIO {
+  @AutoLog
+  public static class VisionIOInputs {
+    public String cameraName;
+    public boolean hasTarget;
+    public boolean isTargetAprilTag;
+    public double targetX;
+    public double targetY;
+    public double targetArea;
+    public double targetSkew;
+    public double targetLatency;
+    public double FId;
+  }
+
+  public default void updateInputs(VisionIOInputs inputs) {}
+
+  public void initPhotonPoseEstimator(
+      Transform3d cameraToRobot, AprilTagFieldLayout tagLayout, PoseStrategy poseStrategy);
+
+  public PhotonPipelineResult getPipelineResult();
+
+  public Optional<EstimatedRobotPose> getEstimatedRobotPose();
+}

src/main/java/frc/robot/subsystems/vision/VisionIOCamera.java

@@ -0,0 +1,99 @@
+package frc.robot.subsystems.vision;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform3d;
+import frc.robot.constants.Constants.Vision;
+
+import java.util.Optional;
+
+import javax.xml.crypto.dsig.Transform;
+
+import org.photonvision.EstimatedRobotPose;
+import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonUtils;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class VisionIOCamera implements VisionIO {
+  public String m_cameraName;
+  public PhotonCamera m_camera;
+  public Transform3d m_cameraToRobot;
+  private PhotonPoseEstimator m_poseEstimator;
+  public AprilTagFieldLayout m_tagLayout;
+  public PoseStrategy m_poseStrategy;
+  private Optional<Pose2d> curPose;
+  private PhotonPipelineResult m_result;
+
+  public VisionIOCamera(String cameraName) {
+    m_cameraName = cameraName;
+    m_camera = new PhotonCamera(m_cameraName);
+  }
+
+  @Override
+  public void initPhotonPoseEstimator(
+      Transform3d cameraToRobot, AprilTagFieldLayout tagLayout, PoseStrategy poseStrategy) {
+    m_tagLayout = tagLayout;
+    m_poseStrategy = poseStrategy;
+    m_poseEstimator = new PhotonPoseEstimator(tagLayout, poseStrategy, m_camera, cameraToRobot);
+  }
+
+  public void initPoseEstimator(
+    Transform3d cameraToRobot, AprilTagFieldLayout tagLayout, PoseStrategy poseStrategy) {
+      m_tagLayout = tagLayout;
+      m_poseStrategy = poseStrategy;
+      m_cameraToRobot = cameraToRobot;
+
+  }
+
+  @Override
+  public PhotonPipelineResult getPipelineResult() {
+
+    return m_result;
+  }
+
+  // @Override
+  // public Optional<EstimatedRobotPose> getEstimatedRobotPose() {
+  //   if (m_poseEstimator != null) {
+  //     curPose = m_poseEstimator.update(getPipelineResult());
+  //     return curPose;
+  //   } else {
+  //     return Optional.empty();
+  //   }
+  // }
+
+  @Override
+  public Optional<Pose2d> getEstimatedRobotPose() {
+    if (m_result.hasTargets()) {
+      PhotonTrackedTarget target = m_result.getBestTarget();
+      Pose3d robotPose = PhotonUtils.estimateFieldToRobotAprilTag(
+        target.getBestCameraToTarget(),
+        m_tagLayout.getTagPose(target.getFiducialId()).get(),
+        m_cameraToRobot
+      );
+      curPose = Optional.of(robotPose.toPose2d());
+      return curPose;
+    }
+    return Optional.empty();
+  }
+
+  @Override
+  public void updateInputs(VisionIOInputs inputs) {
+    inputs.cameraName = m_cameraName;
+    m_result = m_camera.getLatestResult();
+    inputs.hasTarget = m_result.hasTargets();
+    inputs.isTargetAprilTag = m_result.hasTargets() && true;
+    if (m_result.hasTargets()) {
+      PhotonTrackedTarget target = m_result.getBestTarget();
+      inputs.targetX = target.getPitch();
+      inputs.targetY = target.getYaw();
+      inputs.targetArea = target.getArea();
+      inputs.targetSkew = target.getSkew();
+      inputs.FId = target.getFiducialId();
+    }
+  }
+}

src/main/java/frc/robot/util/PoseEstimator.java

@@ -30,4 +30,8 @@ public void resetPose(Pose2d pose) {
   public void addDriveData(Twist2d twist) {
     this.pose = pose.exp(twist);
   }
+
+  // public void addVisionMeasurements(EstimatedRobotPose PhotonEstimatedPose) {
+  //   this.pose = pose.exp
+  // }
 }

vendordeps/photonlib.json

@@ -0,0 +1,57 @@
+{
+  "fileName": "photonlib.json",
+  "name": "photonlib",
+  "version": "v2024.1.2",
+  "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004",
+  "frcYear": "2024",
+  "mavenUrls": [
+    "https://maven.photonvision.org/repository/internal",
+    "https://maven.photonvision.org/repository/snapshots"
+  ],
+  "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/PhotonLib-json/1.0/PhotonLib-json-1.0.json",
+  "jniDependencies": [],
+  "cppDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-cpp",
+      "version": "v2024.1.2",
+      "libName": "photonlib",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-cpp",
+      "version": "v2024.1.2",
+      "libName": "photontargeting",
+      "headerClassifier": "headers",
+      "sharedLibrary": true,
+      "skipInvalidPlatforms": true,
+      "binaryPlatforms": [
+        "windowsx86-64",
+        "linuxathena",
+        "linuxx86-64",
+        "osxuniversal"
+      ]
+    }
+  ],
+  "javaDependencies": [
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photonlib-java",
+      "version": "v2024.1.2"
+    },
+    {
+      "groupId": "org.photonvision",
+      "artifactId": "photontargeting-java",
+      "version": "v2024.1.2"
+    }
+  ]
+}