auto align command help

package frc.robot.commands;

import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest.RobotCentric;
import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
import com.ctre.phoenix6.controls.DriveRequestType;

import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
import edu.wpi.first.wpilibj2.command.Command;

import java.util.function.Supplier;

import frc.robot.subsystems.CommandSwerveDrivetrain;
import frc.robot.LimelightHelpers; // make sure you have this helper class

public class DriveToPoseCommand extends Command {
    private final CommandSwerveDrivetrain drivetrain;
    private final Supplier<Pose2d> targetPoseSupplier;

    private final RobotCentric robotCentric = new RobotCentric()
            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);

    private final double kP_xy = 1.5;
    private final double kP_theta = 4.0;
    private final double toleranceMeters = 0.05;
    private final double toleranceDegrees = 2.0;

    public DriveToPoseCommand(CommandSwerveDrivetrain drivetrain, Supplier<Pose2d> targetPoseSupplier) {
        this.drivetrain = drivetrain;
        this.targetPoseSupplier = targetPoseSupplier;
        addRequirements(drivetrain);
    }

    @Override
    public void execute() {
        Pose2d currentPose = getLimelightPose();
        Pose2d targetPose = targetPoseSupplier.get();

        Translation2d error = targetPose.getTranslation().minus(currentPose.getTranslation());
        double errorTheta = targetPose.getRotation().minus(currentPose.getRotation()).getDegrees();

        double vx = kP_xy * error.getX();
        double vy = kP_xy * error.getY();
        double omega = kP_theta * Math.toRadians(errorTheta);

        drivetrain.setControl(
            robotCentric
                .withVelocityX(vx)
                .withVelocityY(vy)
                .withRotationalRate(omega)
        );
    }

    @Override
    public boolean isFinished() {
        Pose2d currentPose = getLimelightPose();
        Pose2d targetPose = targetPoseSupplier.get();

        boolean posClose = currentPose.getTranslation().getDistance(targetPose.getTranslation()) < toleranceMeters;
        boolean angleClose = Math.abs(currentPose.getRotation().minus(targetPose.getRotation()).getDegrees()) < toleranceDegrees;

        return posClose && angleClose;
    }

    @Override
    public void end(boolean interrupted) {
        drivetrain.setControl(
            robotCentric
                .withVelocityX(0)
                .withVelocityY(0)
                .withRotationalRate(0)
        );
    }

    private Pose2d getLimelightPose() {
        double[] botpose = LimelightHelpers.getBotPose("limelight");
        double x = botpose[0];
        double y = botpose[1];
        double yaw = botpose[5]; // degrees

        return new Pose2d(x, y, Rotation2d.fromDegrees(yaw));
    }
}