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                1,
037                Drive.POWER, 
038                Swerve.Constraints.MAX_VELOCITY_M_PER_S, 
039                Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED, 
040                Drive.RC);
041        this.driver = driver;
042        this.targetPose = targetPose;
043        addRequirements(swerve);
044    }
045
046    static {
047        swerve = CommandSwerveDrivetrain.getInstance();
048    }
049
050    public Rotation2d getTargetAngle() {
051        Pose2d currentPose = swerve.getPose();
052        double atan = Math.atan2(
053                targetPose.get().getY() - currentPose.getY(),
054                targetPose.get().getX() - currentPose.getX());
055        return new Rotation2d(atan);
056    }
057
058    @Override
059    public void execute() {
060        speed.update();
061
062        swerve.setControl(
063                new SwerveRequest.FieldCentricFacingAngle()
064                        .withVelocityX(speed.get().getX())
065                        .withVelocityY(speed.get().getY())
066                        .withTargetDirection(getTargetAngle())
067                        .withHeadingPID(Alignment.akP, Alignment.akI, Alignment.akD));
068        DogLog.log("Swerve/targetAngle", getTargetAngle().getDegrees());
069        DogLog.log("Swerve/Target Pose X", targetPose.get().getX());
070        DogLog.log("Swerve/Target Pose Y", targetPose.get().getY());
071    }
072}