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;
007
008import com.ctre.phoenix6.swerve.SwerveRequest;
009import com.stuypulse.robot.constants.Gains;
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.Rotation2d;
017import edu.wpi.first.wpilibj2.command.Command;
018import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
019
020public class SwerveDriveRotate extends Command {
021
022    private final CommandSwerveDrivetrain swerve;
023
024    private Rotation2d rotation;
025
026    private CommandXboxController driver;
027
028    private final DriveInputProcessor speed;
029
030    public SwerveDriveRotate(CommandXboxController driver, Rotation2d rotation) {
031        this.swerve = CommandSwerveDrivetrain.getInstance();
032        this.rotation = rotation;
033        this.driver = driver;
034        this.speed = new DriveInputProcessor(
035                driver, 
036                Drive.DEADBAND, 
037                1,
038                Drive.POWER, 
039                Swerve.Constraints.MAX_VELOCITY_M_PER_S, 
040                Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED, 
041                Drive.RC);
042        addRequirements(swerve);
043    }
044
045    @Override
046    public void execute() {
047        speed.update();
048
049        SwerveRequest request = new SwerveRequest.FieldCentricFacingAngle()
050                .withTargetDirection(rotation)
051                .withVelocityX(speed.get().getX())
052                .withVelocityY(speed.get().getY())
053                .withHeadingPID(
054                        Gains.Swerve.Alignment.akP, Gains.Swerve.Alignment.akI, Gains.Swerve.Alignment.akD);
055        swerve.setControl(request);
056        DogLog.log(
057                "Swerve/Angle Minus Target Angle",
058                swerve.getPose().getRotation().minus(rotation).getDegrees());
059        DogLog.log("Swerve/Facing Angle", swerve.getPose().getRotation().getDegrees());
060    }
061}