Camera.java

package frc.robot.subsystems;

import edu.wpi.first.networktables.NetworkTable;
import edu.wpi.first.networktables.NetworkTableInstance;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Rotation3d;
import edu.wpi.first.math.geometry.Transform3d;
import java.io.IOException;
import java.util.Optional;

public class Camera extends SubsystemBase {

    private NetworkTable table;

    // TODO: these dont really need to be long, value will not exceed 320
    private long x;
    private long y;
    private long width;
    private long height;
    private boolean lock;
    private long tagId;
    private double distanceCm;
    private double angleOffset;
    private AprilTagFieldLayout fieldLayout;
    private Pose2d estimatedPose;

    public Camera() {
        table = NetworkTableInstance.getDefault().getTable("datatable");
        try {
            fieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2025ReefscapeAndyMark.m_resourceFile);
        } catch (IOException e) {
            // Handle exception
            System.err.println("Failed to load AprilTag field layout: " + e.getMessage());
        }
    }

    @Override
    public void periodic() {
        x = table.getEntry("x").getInteger(0);
        SmartDashboard.putNumber("camera x", x);
        y = table.getEntry("y").getInteger(0);
        SmartDashboard.putNumber("camera y", y);
        width = table.getEntry("width").getInteger(0);
        SmartDashboard.putNumber("camera width", width);
        height = table.getEntry("height").getInteger(0);
        SmartDashboard.putNumber("camera height", height);
        lock = table.getEntry("lock").getBoolean(true);
        SmartDashboard.putBoolean("has lock", lock);
        
        tagId = table.getEntry("tag_id").getInteger(0);
        distanceCm = table.getEntry("distance_cm").getDouble(0);
        angleOffset = table.getEntry("angle_offset").getDouble(0);
        
        SmartDashboard.putNumber("AprilTag ID", tagId);
        SmartDashboard.putNumber("Distance (cm)", distanceCm);
        SmartDashboard.putNumber("Angle Offset", angleOffset);

        if (lock && tagId > 0) {
            Optional<Pose3d> tagPose = fieldLayout.getTagPose((int)tagId);
            if (tagPose.isPresent()) {
                // Calculate robot position using AprilTag data
                double focalLength = 320.0; // Example value, needs calibration
                double tagSize = 0.166; // Tag size in meters
                
                // Calculate distance using the data from the Raspberry Pi
                double distance = distanceCm / 100.0; // Convert cm to meters
                
                // Calculate angle based on the angleOffset from the Pi
                double angleRad = Math.toRadians(angleOffset);
                
                // Create the transform
                Transform3d cameraToTag = new Transform3d(
                    distance, 0, 0,
                    new Rotation3d(0, 0, angleRad)
                );
                
                // Camera position on robot - adjust for your robot
                Transform3d robotToCamera = new Transform3d(
                    0.25, 0, 0.5,
                    new Rotation3d()
                );
                
                // Calculate robot pose
                Pose3d cameraPose = tagPose.get().transformBy(cameraToTag.inverse());
                Pose3d robotPose = cameraPose.transformBy(robotToCamera.inverse());
                estimatedPose = robotPose.toPose2d();
                
                SmartDashboard.putNumber("Pose X", estimatedPose.getX());
                SmartDashboard.putNumber("Pose Y", estimatedPose.getY());
                SmartDashboard.putNumber("Pose Angle", estimatedPose.getRotation().getDegrees());
            }
        }
    }

    public long getX() {
        return x;
    }

    public long getY() {
        return y;
    }

    public long getWidth() {
        return width;
    }

    public long getHeight() {
        return height;
    }

    public boolean isLocked() {
        return lock;
    }

    public long getTagId() {
        return tagId;
    }

    public double getDistanceCm() {
        return distanceCm;
    }

    public double getAngleOffset() {
        return angleOffset;
    }

    public Pose2d getEstimatedPose() {
        return estimatedPose;
    }
}