VisionIOHardware.java

package frc.robot.subsystems.vision;

import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import frc.robot.config.LimelightConfigs;
import frc.robot.robotcontainer.enums.MatchPeriod;
import frc.robot.util.HubTracker;
import frc.robot.util.LimelightHelpers;
import frc.robot.util.LimelightHelpers.PoseEstimate;
import java.util.function.Supplier;

public class VisionIOHardware implements VisionIO {

  private LimelightConfigs shooterCamera;
  private LimelightConfigs rightCamera;
  private LimelightConfigs leftCamera;
  private Supplier<Rotation2d> gyroRotation;

  public VisionIOHardware(
      LimelightConfigs shooterCameraConfig,
      LimelightConfigs rightCameraConfig,
      LimelightConfigs leftCameraConfig,
      Supplier<Rotation2d> gyroRotation) {
    this.shooterCamera = shooterCameraConfig;
    this.rightCamera = rightCameraConfig;
    this.leftCamera = leftCameraConfig;
    this.shooterCamera.initializeTable();
    this.rightCamera.initializeTable();
    this.leftCamera.initializeTable();

    this.gyroRotation = gyroRotation;

    initialize();
  }

  public void initialize() {
    initializeCamera(shooterCamera);
    initializeCamera(rightCamera);
    initializeCamera(leftCamera);
  }

  public void initializeCamera(LimelightConfigs camera) {
    camera.table.getEntry("imumode_set").setInteger(0);
    // double[] doublePose = {
    //   camera.position.getZ(), // forward
    //   -camera.position.getY(), // sideways
    //   camera.position.getX(), // upward
    //   camera.position.getRotation().getX(), // roll
    //   camera.position.getRotation().getY(), // pitch
    //   camera.position.getRotation().getZ() // yaw
    // };
    // camera.table.getEntry("camerapose_robotspace_set").setDoubleArray(doublePose);
  }

  public void throttleCameras(int frames) {
    shooterCamera.table.getEntry("throttle_set").setNumber(0);
    rightCamera.table.getEntry("throttle_set").setNumber(frames);
    leftCamera.table.getEntry("throttle_set").setNumber(frames);
  }

  public void updateInputs(VisionIOInputs inputs) {
    Rotation2d gyroYaw = gyroRotation.get();
    readCamera(shooterCamera, inputs.shooterCamera, gyroYaw);
    readCamera(rightCamera, inputs.rightCamera, gyroYaw);
    readCamera(leftCamera, inputs.leftCamera, gyroYaw);
  }

  public void readCamera(
      LimelightConfigs camera, VisionIOInputs.CameraInputs inputs, Rotation2d gyroYaw) {
    // Update camera heartbeat
    double htbeat = LimelightHelpers.getHeartbeat(inputs.cameraName);
    if (htbeat > inputs.heartbeat) {
      inputs.heartbeat = htbeat;
      inputs.connected = true;
    } else {
      inputs.connected = false;
      return;
    }

    // Update values
    inputs.seesTarget = camera.table.getEntry("tv").getInteger(0) == 1;
    if (inputs.seesTarget) {
      PoseEstimate megatag1 = null;
      // inputs.tagID = (int) LimelightHelpers.getFiducialID(camera.name);
      // Logger.recordOutput("Vision/" + inputs.cameraName, gyroYaw.getDegrees());
      LimelightHelpers.SetRobotOrientation(
          camera.name, gyroYaw.getDegrees(), 0, 0, 0, 0, 0); // other values unnecessary
      PoseEstimate megatag2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(camera.name);
      if (DriverStation.isDisabled()) { // Only use megatag1 while disabled to update gyro
        megatag1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(camera.name);
      }
      inputs.newPose = megatag2 != null;
      inputs.newPoseMtag1 = megatag1 != null;

      inputs.stdDev = camera.table.getEntry("stddevs").getDoubleArray(new double[12]);

      if (HubTracker.getCurrentMatchTime() <= 0.5
          && HubTracker.getMatchPeriod() == MatchPeriod.ENDGAME
          && !inputs.hasRecorded) { // Save limelight replays at the end of real matches
        LimelightHelpers.triggerRewindCapture(inputs.cameraName, 165);
        inputs.hasRecorded = true;
      }

      if (megatag2 != null) {
        inputs.poseEstimate = megatag2;
      }

      if (megatag1 != null) {
        inputs.poseEstimateMtag1 = megatag1;
      }

      // inputs.tagID = (int) this.shooterCamera.table.getEntry("tid").getDouble(-1);
      // inputs.tx = camera.table.getEntry("tx").getDouble(0);
      // double[] tagDistance =
      //     camera.table.getEntry("targetpose_robotspace").getDoubleArray(new double[6]);
      // Translation3d robotCentricTranslation =
      //     new Translation3d(
      //         tagDistance[2], // Z distance
      //         -tagDistance[0] - camera.position.getY(), // inverted Y
      //         -tagDistance[1] // inverted X
      //         );
      // Translation3d fieldTranslation = robotCentricTranslation.rotateBy(new Rotation3d(gyroYaw));
      // Rotation3d robotCentricRotation =
      //     new Rotation3d(
      //         Degrees.of(tagDistance[5]), Degrees.of(tagDistance[3]),
      // Degrees.of(tagDistance[4]));
      // Rotation3d fieldRotation = robotCentricRotation.rotateBy(new Rotation3d(gyroYaw));

      // Transform3d fieldCentricTransform = new Transform3d(fieldTranslation, fieldRotation);

      // inputs.tagDistance = fieldCentricTransform;

    } else { // no targets seen
      // inputs.tagID = -1;
      // inputs.tx = 0.0;
      inputs.newPose = false;
      // inputs.tagDistance = Transform3d.kZero; // Persist old values
    }
  }
}