Limelight.java

package frc.robot.subsystems;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;

public class Limelight extends SubsystemBase {

    NetworkTable networkTable;
    private NetworkTableEntry targetEntry;
    private NetworkTableEntry horizontalAngleOffSet;
    private NetworkTableEntry verticalAngleOffSet;
    private NetworkTableEntry ta;
    private NetworkTableEntry ledMode;

    private boolean target;
    private double x;
    private double y;
    private double area;


    public Limelight() {

        networkTable = NetworkTableInstance.getDefault().getTable("Limelight");

        targetEntry = networkTable.getEntry("tv");// Whether the limelight has any valid targets (0 or 1)
        horizontalAngleOffSet = networkTable.getEntry("tx"); // Horizontal Offset From Crosshair To Target (-27 degrees
                                                             // to 27 degrees)
        verticalAngleOffSet = networkTable.getEntry("ty"); // Vertical Offset From Crosshair To Target (-20.5 degrees to
                                                           // 20.5 degrees)
        ta = networkTable.getEntry("ta"); // Target Area (0% of image to 100% of image
        ledMode = networkTable.getEntry("ledMode");
        
    }

    /**
     * @return a horizontal angle from -27 to 27 between the target and the camera
     */
    public double getHorizontalAngle() {
        return horizontalAngleOffSet.getDouble(0);
    }

    /** @return a vertical angle from -27 to 27 between the target and the camera */
    public double getVerticalAngle() {
        return verticalAngleOffSet.getDouble(0);
    }

    /** @return If the limelight is connected */
    public boolean connected() {
        return networkTable.getEntry("tx").exists();
    }

    /** @return If the limelight has found a target */
    public boolean canSeeTarget() {
        return targetEntry.getNumber(0).intValue() > 0;
    }

    /** @return If the limelight is on target */
    public boolean onTarget() {
        return canSeeTarget() && getHorizontalAngle() < Constants.APPROXIMATE_ANGLE;
    }

    // returns the size of the target
    public double getTargetArea() {
        return ta.getNumber(0).doubleValue();
    }

    public double calculateDistance() {
        return (Constants.PORT_HEIGHT - Constants.HEIGHT_ABOVE_GROUND)
                / Math.tan(Math.toRadians(y) + Math.toRadians(Constants.MOUNTED_ANGLE));
    }

    public void turnOn() {
        ledMode.setDouble(3);
    }

    public void turnOff() {
        ledMode.setDouble(1);
    }

    public void periodic() {

        target = targetEntry.getBoolean(false);
        x = horizontalAngleOffSet.getDouble(0.0);
        y = verticalAngleOffSet.getDouble(0.0);
        area = ta.getDouble(0.0);

        SmartDashboard.putNumber("Horizontal Error", x);

    }

}