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                                1,
034                Drive.POWER, 
035                Swerve.Constraints.MAX_VELOCITY_M_PER_S, 
036                Swerve.Constraints.MAX_ACCEL_M_PER_S_SQUARED, 
037                Drive.RC);
038                turn = new DriveTurnInputProcessor(
039                        driver, 
040                        Turn.DEADBAND, 
041                        Turn.POWER, 
042                        Swerve.Constraints.MAX_ANGULAR_VEL_RAD_PER_S, Turn.RC);
043                this.driver = driver;
044                addRequirements(swerve);
045        }
046
047        @Override
048        public void execute() {
049                speed.update();
050                turn.update();
051
052                swerve.setControl(
053                                swerve
054                                                .getFieldCentricSwerveRequest()
055                                                .withVelocityX(speed.get().getX())
056                                                .withVelocityY(speed.get().getY())
057                                                .withRotationalRate(-turn.get()));
058        }
059}