drive to pose command help 2

package frc.robot.commands;

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

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

import java.util.function.Supplier;

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

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

    // tuning constants
    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() {
        updateOdometry();
        Pose2d currentPose = drivetrain.getState().Pose;
        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)
        );
    }

    public void updateOdometry() {
    var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue("limelight");
    if (llMeasurement != null) {
            m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose,        Utils.fpgaToCurrentTime(llMeasurement.timestampSeconds));
        }

    }

    @Override
    public boolean isFinished() {
        Pose2d currentPose = drivetrain.getState().Pose;
        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)
        );
    }
}