Limelight撰寫

1. 於 src/main/java/frc/robot/subsystems 創建 Limelight.java （有使用IDashboard）
   NetworkTable 讀取Limelight
   NetworkTableEntry 讀取Limelight值（tx、ty為度數）
   Limelight各項值

public class Limelight extends SubsystemBase implements IDashboardProvider {
    private final NetworkTable table;
    private final NetworkTableEntry tx;
    private final NetworkTableEntry ty;
    private final NetworkTableEntry tid;
    private double distanceToGoalVerticalMeters;
    private double distanceToGoalHorizontalMeters;

    public Limelight() {
        this.registerDashboard();
        this.table = NetworkTableInstance.getDefault().getTable("limelight");
        this.tx = this.table.getEntry("tx");
        this.ty = this.table.getEntry("ty");
        this.tid = this.table.getEntry("tid");
        this.table.getEntry("ledMode").setNumber(LimelightCamera.ledMode);
        this.table.getEntry("camMode").setNumber(LimelightCamera.camMode);
        this.table.getEntry("crop").setDoubleArray(LimelightCamera.cameraPose);
    }
}

2. 垂直計算距離
   a1 VerticalOffset 與AprilTag角度
   a2 MountAngleDeg Limelight角度
   h1 LensHeightMeters Limelight與地板高度
   h2 GoalHeightMeters AprilTag與地板高度

tan(a1+a2) = (h2-h1) / d
d = (h2-h1) / tan(a1+a2)

public double getDistanceToGoalVerticalMeters() {
    double verticalOffset = this.ty.getDouble(0.0);

    double mountAngleDeg = LimelightConstants.MOUNT_ANGLE_DEG;
    double lensHeightMeters = LimelightConstants.LENS_HEIGHT_METERS;
    double goalHeightMeters = LimelightConstants.GOAL_HEIGHT_METERS;

    double angleToGoalDeg = mountAngleDeg + verticalOffset;
    double angleToGoalRad = angleToGoalDeg * (Math.PI / 180.0);
    this.distanceToGoalVerticalMeters = Math.abs((goalHeightMeters - lensHeightMeters) / Math.tan(angleToGoalRad));

    return this.distanceToGoalVerticalMeters;
}

3. 計算水平距離（俯視圖）
   a1 HorizontalOffset 與AprilTag夾角
   d distanceToGoalVerticalMeters 與AprilTag距離
   h distanceToGoalHorizontalMeters 與Limelight水平距離
   h = tan(a1) * d

public double getDistanceToGoalHorizontalMeters(double distanceToGoalVerticalMeters) {
    if (distanceToGoalVerticalMeters == -1) {
        distanceToGoalVerticalMeters = this.getDistanceToGoalVerticalMeters();
    }
    double horizontalOffset = this.tx.getDouble(0.0);

    double horizontalOffsetRad = horizontalOffset * (Math.PI / 180.0);

    this.distanceToGoalHorizontalMeters = (Math.tan(horizontalOffsetRad) * distanceToGoalVerticalMeters) - LimelightConstants.HORIZONTAL_OFFSET_METERS;
    return this.distanceToGoalHorizontalMeters;
}

4. 取得AprilTag ID

public double getAprilTagId() {
    return this.tid.getDouble(0.0);
}