# Limelight撰寫
1. 於 `src/main/java/frc/robot/subsystems` 創建 `Limelight.java` （有使用IDashboard）
`NetworkTable` 讀取Limelight
`NetworkTableEntry` 讀取Limelight值（tx、ty為度數）
[Limelight各項值](https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api)

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