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.stuypulse.robot.constants.Settings.Driver.Drive;
009import com.stuypulse.robot.constants.Settings.Driver.Turn;
010import com.stuypulse.robot.constants.Settings.Swerve;
011import com.stuypulse.robot.subsystems.swerve.CommandSwerveDrivetrain;
012import com.stuypulse.robot.util.swerve.swerveinput.DriveInputProcessor;
013import com.stuypulse.robot.util.swerve.swerveinput.DriveTurnInputProcessor;
014
015import edu.wpi.first.wpilibj2.command.Command;
016import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
017
018public class SwerveDriveDrive extends Command {
019
020        private final CommandSwerveDrivetrain swerve;
021
022        private final CommandXboxController driver;
023
024        private final DriveInputProcessor speed;
025
026        private final DriveTurnInputProcessor turn;
027
028        public SwerveDriveDrive(CommandXboxController driver) {
029                swerve = CommandSwerveDrivetrain.getInstance();
030                this.speed = new DriveInputProcessor(
031                driver, 
032                Drive.DEADBAND, 
033                Drive.POWER, 
034                Swerve.Constraints.MAX_VELOCITY_M_PER_S, 
035                Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED, 
036                Drive.RC);
037                turn = new DriveTurnInputProcessor(
038                        driver, 
039                        Turn.DEADBAND, 
040                        Turn.POWER, 
041                        Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, Turn.RC);
042                this.driver = driver;
043                addRequirements(swerve);
044        }
045
046        @Override
047        public void execute() {
048                speed.update();
049                turn.update();
050
051                swerve.setControl(
052                                swerve
053                                                .getFieldCentricSwerveRequest()
054                                                .withVelocityX(speed.get().getX())
055                                                .withVelocityY(speed.get().getY())
056                                                .withRotationalRate(-turn.get()));
057        }
058}