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                Drive.POWER, 
038                Swerve.Constraints.MAX_VELOCITY_M_PER_S, 
039                Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED, 
040                Drive.RC);
041        addRequirements(swerve);
042    }
043
044    @Override
045    public void execute() {
046        speed.update();
047
048        SwerveRequest request = new SwerveRequest.FieldCentricFacingAngle()
049                .withTargetDirection(rotation)
050                .withVelocityX(speed.get().getX())
051                .withVelocityY(speed.get().getY())
052                .withHeadingPID(
053                        Gains.Swerve.Alignment.akP, Gains.Swerve.Alignment.akI, Gains.Swerve.Alignment.akD);
054        swerve.setControl(request);
055        DogLog.log(
056                "Swerve/Angle Minus Target Angle",
057                swerve.getPose().getRotation().minus(rotation).getDegrees());
058        DogLog.log("Swerve/Facing Angle", swerve.getPose().getRotation().getDegrees());
059    }
060}