001/************************* PROJECT RON *************************/
002/* Copyright (c) 2026 StuyPulse Robotics. All rights reserved. */
003/* Use of this source code is governed by an MIT-style license */
004/* that can be found in the repository LICENSE file.           */
005/***************************************************************/
006package com.stuypulse.robot.commands.swerve.driveAligned;
007
008import com.ctre.phoenix6.swerve.SwerveRequest;
009import com.stuypulse.robot.constants.Gains.Swerve.Alignment;
010import com.stuypulse.robot.constants.Settings.Driver.Drive;
011import com.stuypulse.robot.constants.Settings.Swerve;
012import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
013import com.stuypulse.robot.util.swerve.swerveinput.DriveInputProcessor;
014
015import dev.doglog.DogLog;
016import edu.wpi.first.math.geometry.Pose2d;
017import edu.wpi.first.math.geometry.Rotation2d;
018import edu.wpi.first.wpilibj2.command.Command;
019import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
020import java.util.function.Supplier;
021
022public class SwerveDriveDriveWhileAligned extends Command {
023
024    protected static final CommandSwerveDrivetrain swerve;
025
026    private final CommandXboxController driver;
027
028    private final DriveInputProcessor speed;
029
030    private final Supplier<Pose2d> targetPose;
031
032    public SwerveDriveDriveWhileAligned(CommandXboxController driver, Supplier<Pose2d> targetPose) {
033        this.speed = new DriveInputProcessor(
034                driver, 
035                Drive.DEADBAND, 
036                Drive.POWER, 
037                Swerve.Constraints.MAX_VELOCITY_M_PER_S, 
038                Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED, 
039                Drive.RC);
040        this.driver = driver;
041        this.targetPose = targetPose;
042        addRequirements(swerve);
043    }
044
045    static {
046        swerve = CommandSwerveDrivetrain.getInstance();
047    }
048
049    public Rotation2d getTargetAngle() {
050        Pose2d currentPose = swerve.getPose();
051        double atan = Math.atan2(
052                targetPose.get().getY() - currentPose.getY(),
053                targetPose.get().getX() - currentPose.getX());
054        return new Rotation2d(atan);
055    }
056
057    @Override
058    public void execute() {
059        speed.update();
060
061        swerve.setControl(
062                new SwerveRequest.FieldCentricFacingAngle()
063                        .withVelocityX(speed.get().getX())
064                        .withVelocityY(speed.get().getY())
065                        .withTargetDirection(getTargetAngle())
066                        .withHeadingPID(Alignment.akP, Alignment.akI, Alignment.akD));
067        DogLog.log("Swerve/targetAngle", getTargetAngle().getDegrees());
068        DogLog.log("Swerve/Target Pose X", targetPose.get().getX());
069        DogLog.log("Swerve/Target Pose Y", targetPose.get().getY());
070    }
071}