Removing REV template hot hot garbage programming practices

src/main/java/frc/robot/Configs.java

@@ -1,56 +0,0 @@
-package frc.robot;
-
-import com.revrobotics.spark.config.SparkMaxConfig;
-import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
-import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
-
-import frc.robot.Constants.ModuleConstants;
-
-public final class Configs {
-    public static final class MAXSwerveModule {
-        public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
-        public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
-
-        static {
-            // Use module constants to calculate conversion factors and feed forward gain.
-            double drivingFactor = ModuleConstants.kWheelDiameterMeters * Math.PI
-                    / ModuleConstants.kDrivingMotorReduction;
-            double turningFactor = 2 * Math.PI;
-            double drivingVelocityFeedForward = 1 / ModuleConstants.kDriveWheelFreeSpeedRps;
-
-            drivingConfig
-                    .idleMode(IdleMode.kBrake)
-                    .smartCurrentLimit(50);
-            drivingConfig.encoder
-                    .positionConversionFactor(drivingFactor) // meters
-                    .velocityConversionFactor(drivingFactor / 60.0); // meters per second
-            drivingConfig.closedLoop
-                    .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
-                    // These are example gains you may need to them for your own robot!
-                    .pid(0.04, 0, 0)
-                    .velocityFF(drivingVelocityFeedForward)
-                    .outputRange(-1, 1);
-
-            turningConfig
-                    .idleMode(IdleMode.kBrake)
-                    .smartCurrentLimit(20);
-            turningConfig.absoluteEncoder
-                    // Invert the turning encoder, since the output shaft rotates in the opposite
-                    // direction of the steering motor in the MAXSwerve Module.
-                    .inverted(true)
-                    .positionConversionFactor(turningFactor) // radians
-                    .velocityConversionFactor(turningFactor / 60.0); // radians per second
-            turningConfig.closedLoop
-                    .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
-                    // These are example gains you may need to them for your own robot!
-                    .pid(1, 0, 0)
-                    .outputRange(-1, 1)
-                    // Enable PID wrap around for the turning motor. This will allow the PID
-                    // controller to go through 0 to get to the setpoint i.e. going from 350 degrees
-                    // to 10 degrees will go through 0 rather than the other direction which is a
-                    // longer route.
-                    .positionWrappingEnabled(true)
-                    .positionWrappingInputRange(0, turningFactor);
-        }
-    }
-}

src/main/java/frc/robot/Constants.java

@@ -1,102 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc.robot;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.math.util.Units;
-
-/**
- * The Constants class provides a convenient place for teams to hold robot-wide
- * numerical or boolean
- * constants. This class should not be used for any other purpose. All constants
- * should be declared
- * globally (i.e. public static). Do not put anything functional in this class.
- *
- * <p>
- * It is advised to statically import this class (or one of its inner classes)
- * wherever the
- * constants are needed, to reduce verbosity.
- */
-public final class Constants {
-  public static final class DriveConstants {
-    // Driving Parameters - Note that these are not the maximum capable speeds of
-    // the robot, rather the allowed maximum speeds
-    public static final double kMaxSpeedMetersPerSecond = 4.8;
-    public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
-
-    // Chassis configuration
-    public static final double kTrackWidth = Units.inchesToMeters(26.5);
-    // Distance between centers of right and left wheels on robot
-    public static final double kWheelBase = Units.inchesToMeters(26.5);
-    // Distance between front and back wheels on robot
-    public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
-        new Translation2d(kWheelBase / 2, kTrackWidth / 2),
-        new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
-        new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
-        new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
-
-    // Angular offsets of the modules relative to the chassis in radians
-    public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
-    public static final double kFrontRightChassisAngularOffset = 0;
-    public static final double kBackLeftChassisAngularOffset = Math.PI;
-    public static final double kBackRightChassisAngularOffset = Math.PI / 2;
-
-    // SPARK MAX CAN IDs
-    public static final int kFrontLeftDrivingCanId = 11;
-    public static final int kRearLeftDrivingCanId = 13;
-    public static final int kFrontRightDrivingCanId = 15;
-    public static final int kRearRightDrivingCanId = 17;
-
-    public static final int kFrontLeftTurningCanId = 10;
-    public static final int kRearLeftTurningCanId = 12;
-    public static final int kFrontRightTurningCanId = 14;
-    public static final int kRearRightTurningCanId = 16;
-
-    public static final boolean kGyroReversed = false;
-  }
-
-  public static final class ModuleConstants {
-    // The MAXSwerve module can be configured with one of three pinion gears: 12T,
-    // 13T, or 14T. This changes the drive speed of the module (a pinion gear with
-    // more teeth will result in a robot that drives faster).
-    public static final int kDrivingMotorPinionTeeth = 14;
-
-    // Calculations required for driving motor conversion factors and feed forward
-    public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60;
-    public static final double kWheelDiameterMeters = 0.0762;
-    public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
-    // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15
-    // teeth on the bevel pinion
-    public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
-    public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
-        / kDrivingMotorReduction;
-  }
-
-  public static final class OIConstants {
-    public static final int kDriverControllerPort = 0;
-    public static final double kDriveDeadband = 0.05;
-  }
-
-  public static final class AutoConstants {
-    public static final double kMaxSpeedMetersPerSecond = 3;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
-    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
-    public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
-
-    public static final double kPXController = 1;
-    public static final double kPYController = 1;
-    public static final double kPThetaController = 1;
-
-    // Constraint for the motion profiled robot angle controller
-    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
-        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
-  }
-
-  public static final class NeoMotorConstants {
-    public static final double kFreeSpeedRpm = 5676;
-  }
-}

src/main/java/frc/robot/RobotContainer.java

@@ -4,119 +4,39 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.MathUtil;
+import frc.robot.constants.OIConstants;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.Trajectory;
-import edu.wpi.first.math.trajectory.TrajectoryConfig;
-import edu.wpi.first.math.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.PS4Controller.Button;
-import frc.robot.Constants.AutoConstants;
-import frc.robot.Constants.DriveConstants;
-import frc.robot.Constants.OIConstants;
 import frc.robot.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
-import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-import java.util.List;
 
-/*
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
- */
 public class RobotContainer {
-  // The robot's subsystems
+  private DriveSubsystem drivetrain;
-  private final DriveSubsystem m_robotDrive = new DriveSubsystem();
 
-  // The driver's controller
+  private CommandXboxController driver;
-  XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot. Contains subsystems, OI devices, and commands.
-   */
   public RobotContainer() {
-    // Configure the button bindings
+    drivetrain = new DriveSubsystem();
+
+    driver = new CommandXboxController(OIConstants.kDriverControllerPort);
+    
     configureButtonBindings();
-
-    // Configure default commands
-    m_robotDrive.setDefaultCommand(
-        // The left stick controls translation of the robot.
-        // Turning is controlled by the X axis of the right stick.
-        new RunCommand(
-            () -> m_robotDrive.drive(
-                -MathUtil.applyDeadband(m_driverController.getLeftY(), OIConstants.kDriveDeadband),
-                -MathUtil.applyDeadband(m_driverController.getLeftX(), OIConstants.kDriveDeadband),
-                -MathUtil.applyDeadband(m_driverController.getRightX(), OIConstants.kDriveDeadband),
-                true),
-            m_robotDrive));
   }
 
-  /**
-   * Use this method to define your button->command mappings. Buttons can be
-   * created by
-   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its
-   * subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling
-   * passing it to a
-   * {@link JoystickButton}.
-   */
   private void configureButtonBindings() {
-    new JoystickButton(m_driverController, Button.kR1.value)
+    drivetrain.setDefaultCommand(
-        .whileTrue(new RunCommand(
+        drivetrain.drive(
-            () -> m_robotDrive.setX(),
+            driver::getLeftY, 
-            m_robotDrive));
+            driver::getLeftX, 
+            driver::getRightX, 
+            () -> true
+        )
+    );
+
+    driver.start().whileTrue(drivetrain.setXCommand());
   }
 
-  /**
-   * Use this to pass the autonomous command to the main {@link Robot} class.
-   *
-   * @return the command to run in autonomous
-   */
   public Command getAutonomousCommand() {
-    // Create config for trajectory
+    return new PrintCommand("NO AUTO DEFINED");
-    TrajectoryConfig config = new TrajectoryConfig(
-        AutoConstants.kMaxSpeedMetersPerSecond,
-        AutoConstants.kMaxAccelerationMetersPerSecondSquared)
-        // Add kinematics to ensure max speed is actually obeyed
-        .setKinematics(DriveConstants.kDriveKinematics);
-
-    // An example trajectory to follow. All units in meters.
-    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
-        // Start at the origin facing the +X direction
-        new Pose2d(0, 0, new Rotation2d(0)),
-        // Pass through these two interior waypoints, making an 's' curve path
-        List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
-        // End 3 meters straight ahead of where we started, facing forward
-        new Pose2d(3, 0, new Rotation2d(0)),
-        config);
-
-    var thetaController = new ProfiledPIDController(
-        AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
-    thetaController.enableContinuousInput(-Math.PI, Math.PI);
-
-    SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
-        exampleTrajectory,
-        m_robotDrive::getPose, // Functional interface to feed supplier
-        DriveConstants.kDriveKinematics,
-
-        // Position controllers
-        new PIDController(AutoConstants.kPXController, 0, 0),
-        new PIDController(AutoConstants.kPYController, 0, 0),
-        thetaController,
-        m_robotDrive::setModuleStates,
-        m_robotDrive);
-
-    // Reset odometry to the starting pose of the trajectory.
-    m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
-
-    // Run path following command, then stop at the end.
-    return swerveControllerCommand.andThen(() -> m_robotDrive.drive(0, 0, 0, false));
   }
 }

src/main/java/frc/robot/constants/AutoConstants.java

@@ -0,0 +1,18 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+public class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = 3;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+    public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI;
+
+    public static final double kPXController = 1;
+    public static final double kPYController = 1;
+    public static final double kPThetaController = 1;
+
+    // Constraint for the motion profiled robot angle controller
+    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+}

src/main/java/frc/robot/constants/DriveConstants.java

@@ -0,0 +1,42 @@
+package frc.robot.constants;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+
+public class DriveConstants {
+    // Driving Parameters - Note that these are not the maximum capable speeds of
+    // the robot, rather the allowed maximum speeds
+    public static final double kMaxSpeedMetersPerSecond = 4.8;
+    public static final double kMaxAngularSpeed = 2 * Math.PI; // radians per second
+
+    // Chassis configuration
+    public static final double kTrackWidth = Units.inchesToMeters(26.5);
+    // Distance between centers of right and left wheels on robot
+    public static final double kWheelBase = Units.inchesToMeters(26.5);
+    // Distance between front and back wheels on robot
+    public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
+        new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+        new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+
+    // Angular offsets of the modules relative to the chassis in radians
+    public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
+    public static final double kFrontRightChassisAngularOffset = 0;
+    public static final double kBackLeftChassisAngularOffset = Math.PI;
+    public static final double kBackRightChassisAngularOffset = Math.PI / 2;
+
+    // SPARK MAX CAN IDs
+    public static final int kFrontLeftDrivingCanId = 11;
+    public static final int kRearLeftDrivingCanId = 13;
+    public static final int kFrontRightDrivingCanId = 15;
+    public static final int kRearRightDrivingCanId = 17;
+
+    public static final int kFrontLeftTurningCanId = 10;
+    public static final int kRearLeftTurningCanId = 12;
+    public static final int kFrontRightTurningCanId = 14;
+    public static final int kRearRightTurningCanId = 16;
+
+    public static final boolean kGyroReversed = false;
+  }

src/main/java/frc/robot/constants/ModuleConstants.java

@@ -0,0 +1,66 @@
+package frc.robot.constants;
+
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class ModuleConstants {
+    // The MAXSwerve module can be configured with one of three pinion gears: 12T,
+    // 13T, or 14T. This changes the drive speed of the module (a pinion gear with
+    // more teeth will result in a robot that drives faster).
+    public static final int kDrivingMotorPinionTeeth = 14;
+
+    // Calculations required for driving motor conversion factors and feed forward
+    public static final double kDrivingMotorFreeSpeedRps = NeoMotorConstants.kFreeSpeedRpm / 60;
+    public static final double kWheelDiameterMeters = 0.0762;
+    public static final double kWheelCircumferenceMeters = kWheelDiameterMeters * Math.PI;
+    // 45 teeth on the wheel's bevel gear, 22 teeth on the first-stage spur gear, 15
+    // teeth on the bevel pinion
+    public static final double kDrivingMotorReduction = (45.0 * 22) / (kDrivingMotorPinionTeeth * 15);
+    public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
+        / kDrivingMotorReduction;
+
+    public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
+    public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
+
+    static {
+        // Use module constants to calculate conversion factors and feed forward gain.
+        double drivingFactor = kWheelDiameterMeters * Math.PI / kDrivingMotorReduction;
+        double turningFactor = 2 * Math.PI;
+        double drivingVelocityFeedForward = 1 / kDriveWheelFreeSpeedRps;
+
+        drivingConfig
+            .idleMode(IdleMode.kBrake)
+            .smartCurrentLimit(50);
+        drivingConfig.encoder
+            .positionConversionFactor(drivingFactor) // meters
+            .velocityConversionFactor(drivingFactor / 60.0); // meters per second
+        drivingConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            // These are example gains you may need to them for your own robot!
+            .pid(0.04, 0, 0)
+            .velocityFF(drivingVelocityFeedForward)
+            .outputRange(-1, 1);
+
+        turningConfig
+            .idleMode(IdleMode.kBrake)
+            .smartCurrentLimit(20);
+        turningConfig.absoluteEncoder
+            // Invert the turning encoder, since the output shaft rotates in the opposite
+            // direction of the steering motor in the MAXSwerve Module.
+            .inverted(true)
+            .positionConversionFactor(turningFactor) // radians
+            .velocityConversionFactor(turningFactor / 60.0); // radians per second
+        turningConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
+            // These are example gains you may need to them for your own robot!
+            .pid(1, 0, 0)
+            .outputRange(-1, 1)
+            // Enable PID wrap around for the turning motor. This will allow the PID
+            // controller to go through 0 to get to the setpoint i.e. going from 350 degrees
+            // to 10 degrees will go through 0 rather than the other direction which is a
+            // longer route.
+            .positionWrappingEnabled(true)
+            .positionWrappingInputRange(0, turningFactor);
+    }
+}

src/main/java/frc/robot/constants/NeoMotorConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class NeoMotorConstants {
+    public static final double kFreeSpeedRpm = 5676;
+}

src/main/java/frc/robot/constants/OIConstants.java

@@ -0,0 +1,7 @@
+package frc.robot.constants;
+
+public class OIConstants {
+    public static final int kDriverControllerPort = 0;
+    
+    public static final double kDriveDeadband = 0.05;
+}

src/main/java/frc/robot/subsystems/DriveSubsystem.java

@@ -4,6 +4,10 @@
 
 package frc.robot.subsystems;
 
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -13,168 +17,201 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.ADIS16470_IMU;
 import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
-import frc.robot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.DriveConstants;
+import frc.robot.constants.OIConstants;
 
 public class DriveSubsystem extends SubsystemBase {
-  // Create MAXSwerveModules
+    // Create MAXSwerveModules
-  private final MAXSwerveModule m_frontLeft = new MAXSwerveModule(
+    private MAXSwerveModule m_frontLeft;
-      DriveConstants.kFrontLeftDrivingCanId,
+    private MAXSwerveModule m_frontRight;
-      DriveConstants.kFrontLeftTurningCanId,
+    private MAXSwerveModule m_rearLeft;
-      DriveConstants.kFrontLeftChassisAngularOffset);
+    private MAXSwerveModule m_rearRight;
 
-  private final MAXSwerveModule m_frontRight = new MAXSwerveModule(
+    // The gyro sensor
-      DriveConstants.kFrontRightDrivingCanId,
+    private ADIS16470_IMU m_gyro;
-      DriveConstants.kFrontRightTurningCanId,
-      DriveConstants.kFrontRightChassisAngularOffset);
 
-  private final MAXSwerveModule m_rearLeft = new MAXSwerveModule(
+    // Odometry class for tracking robot pose
-      DriveConstants.kRearLeftDrivingCanId,
+    private SwerveDriveOdometry m_odometry;
-      DriveConstants.kRearLeftTurningCanId,
-      DriveConstants.kBackLeftChassisAngularOffset);
 
-  private final MAXSwerveModule m_rearRight = new MAXSwerveModule(
+    /** Creates a new DriveSubsystem. */
-      DriveConstants.kRearRightDrivingCanId,
+    public DriveSubsystem() {
-      DriveConstants.kRearRightTurningCanId,
+        m_frontLeft = new MAXSwerveModule(
-      DriveConstants.kBackRightChassisAngularOffset);
+            DriveConstants.kFrontLeftDrivingCanId,
+            DriveConstants.kFrontLeftTurningCanId,
+            DriveConstants.kFrontLeftChassisAngularOffset
+        );
 
-  // The gyro sensor
+        m_frontRight = new MAXSwerveModule(
-  private final ADIS16470_IMU m_gyro = new ADIS16470_IMU();
+            DriveConstants.kFrontRightDrivingCanId,
+            DriveConstants.kFrontRightTurningCanId,
+            DriveConstants.kFrontRightChassisAngularOffset
+        );
 
-  // Odometry class for tracking robot pose
+        m_rearLeft = new MAXSwerveModule(
-  SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(
+            DriveConstants.kRearLeftDrivingCanId,
-      DriveConstants.kDriveKinematics,
+            DriveConstants.kRearLeftTurningCanId,
-      Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            DriveConstants.kBackLeftChassisAngularOffset
-      new SwerveModulePosition[] {
+        );
-          m_frontLeft.getPosition(),
-          m_frontRight.getPosition(),
-          m_rearLeft.getPosition(),
-          m_rearRight.getPosition()
-      });
 
-  /** Creates a new DriveSubsystem. */
+        m_rearRight = new MAXSwerveModule(
-  public DriveSubsystem() {
+            DriveConstants.kRearRightDrivingCanId,
-  }
+            DriveConstants.kRearRightTurningCanId,
+            DriveConstants.kBackRightChassisAngularOffset
+        );
 
-  @Override
+        m_gyro = new ADIS16470_IMU();
-  public void periodic() {
+
-    // Update the odometry in the periodic block
+        m_odometry = new SwerveDriveOdometry(
-    m_odometry.update(
+            DriveConstants.kDriveKinematics,
-        Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
-        new SwerveModulePosition[] {
+            new SwerveModulePosition[] {
-            m_frontLeft.getPosition(),
+                m_frontLeft.getPosition(),
-            m_frontRight.getPosition(),
+                m_frontRight.getPosition(),
-            m_rearLeft.getPosition(),
+                m_rearLeft.getPosition(),
-            m_rearRight.getPosition()
+                m_rearRight.getPosition()
         });
-  }
+    }
 
-  /**
+    @Override
-   * Returns the currently-estimated pose of the robot.
+    public void periodic() {
-   *
+        // Update the odometry in the periodic block
-   * @return The pose.
+        m_odometry.update(
-   */
+            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
-  public Pose2d getPose() {
+            new SwerveModulePosition[] {
-    return m_odometry.getPoseMeters();
+                m_frontLeft.getPosition(),
-  }
+                m_frontRight.getPosition(),
+                m_rearLeft.getPosition(),
+                m_rearRight.getPosition()
+        });
+    }
 
-  /**
+    /**
-   * Resets the odometry to the specified pose.
+     * Returns the currently-estimated pose of the robot.
-   *
+     *
-   * @param pose The pose to which to set the odometry.
+     * @return The pose.
-   */
+     */
-  public void resetOdometry(Pose2d pose) {
+    public Pose2d getPose() {
-    m_odometry.resetPosition(
+        return m_odometry.getPoseMeters();
-        Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
+    }
-        new SwerveModulePosition[] {
-            m_frontLeft.getPosition(),
-            m_frontRight.getPosition(),
-            m_rearLeft.getPosition(),
-            m_rearRight.getPosition()
-        },
-        pose);
-  }
 
-  /**
+    /**
-   * Method to drive the robot using joystick info.
+     * Resets the odometry to the specified pose.
-   *
+     *
-   * @param xSpeed        Speed of the robot in the x direction (forward).
+     * @param pose The pose to which to set the odometry.
-   * @param ySpeed        Speed of the robot in the y direction (sideways).
+     */
-   * @param rot           Angular rate of the robot.
+    public void resetOdometry(Pose2d pose) {
-   * @param fieldRelative Whether the provided x and y speeds are relative to the
+        m_odometry.resetPosition(
-   *                      field.
+            Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
-   */
+            new SwerveModulePosition[] {
-  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+                m_frontLeft.getPosition(),
-    // Convert the commanded speeds into the correct units for the drivetrain
+                m_frontRight.getPosition(),
-    double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeedMetersPerSecond;
+                m_rearLeft.getPosition(),
-    double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeedMetersPerSecond;
+                m_rearRight.getPosition()
-    double rotDelivered = rot * DriveConstants.kMaxAngularSpeed;
+            },
+            pose
+        );
+    }
 
-    var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
+    public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rot,
-        fieldRelative
+            BooleanSupplier fieldRelative) {
-            ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
+        return run(() -> {
-                Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
+            drive(
-            : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
+                -MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-    SwerveDriveKinematics.desaturateWheelSpeeds(
+                -MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-        swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
+                -MathUtil.applyDeadband(rot.getAsDouble(), OIConstants.kDriveDeadband),
-    m_frontLeft.setDesiredState(swerveModuleStates[0]);
+                fieldRelative.getAsBoolean()
-    m_frontRight.setDesiredState(swerveModuleStates[1]);
+            );
-    m_rearLeft.setDesiredState(swerveModuleStates[2]);
+        });
-    m_rearRight.setDesiredState(swerveModuleStates[3]);
+    }
-  }
 
-  /**
+    /**
-   * Sets the wheels into an X formation to prevent movement.
+     * Method to drive the robot using joystick info.
-   */
+     *
-  public void setX() {
+     * @param xSpeed        Speed of the robot in the x direction (forward).
-    m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+     * @param ySpeed        Speed of the robot in the y direction (sideways).
-    m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+     * @param rot           Angular rate of the robot.
-    m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+     * @param fieldRelative Whether the provided x and y speeds are relative to the
-    m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+     *                      field.
-  }
+     */
+    public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+        // Convert the commanded speeds into the correct units for the drivetrain
+        double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeedMetersPerSecond;
+        double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeedMetersPerSecond;
+        double rotDelivered = rot * DriveConstants.kMaxAngularSpeed;
 
-  /**
+        var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
-   * Sets the swerve ModuleStates.
+            fieldRelative
-   *
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
-   * @param desiredStates The desired SwerveModule states.
+                        Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
-   */
+                : new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
-  public void setModuleStates(SwerveModuleState[] desiredStates) {
+        SwerveDriveKinematics.desaturateWheelSpeeds(
-    SwerveDriveKinematics.desaturateWheelSpeeds(
+            swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
-        desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
+        m_frontLeft.setDesiredState(swerveModuleStates[0]);
-    m_frontLeft.setDesiredState(desiredStates[0]);
+        m_frontRight.setDesiredState(swerveModuleStates[1]);
-    m_frontRight.setDesiredState(desiredStates[1]);
+        m_rearLeft.setDesiredState(swerveModuleStates[2]);
-    m_rearLeft.setDesiredState(desiredStates[2]);
+        m_rearRight.setDesiredState(swerveModuleStates[3]);
-    m_rearRight.setDesiredState(desiredStates[3]);
+    }
-  }
 
-  /** Resets the drive encoders to currently read a position of 0. */
+    public Command setXCommand() {
-  public void resetEncoders() {
+        return run(() -> {
-    m_frontLeft.resetEncoders();
+            setX();
-    m_rearLeft.resetEncoders();
+        });
-    m_frontRight.resetEncoders();
+    }
-    m_rearRight.resetEncoders();
-  }
 
-  /** Zeroes the heading of the robot. */
+    /**
-  public void zeroHeading() {
+     * Sets the wheels into an X formation to prevent movement.
-    m_gyro.reset();
+     */
-  }
+    public void setX() {
+        m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+        m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+        m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
+        m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
+    }
 
-  /**
+    /**
-   * Returns the heading of the robot.
+     * Sets the swerve ModuleStates.
-   *
+     *
-   * @return the robot's heading in degrees, from -180 to 180
+     * @param desiredStates The desired SwerveModule states.
-   */
+     */
-  public double getHeading() {
+    public void setModuleStates(SwerveModuleState[] desiredStates) {
-    return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
+        SwerveDriveKinematics.desaturateWheelSpeeds(
-  }
+            desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
+        m_frontLeft.setDesiredState(desiredStates[0]);
+        m_frontRight.setDesiredState(desiredStates[1]);
+        m_rearLeft.setDesiredState(desiredStates[2]);
+        m_rearRight.setDesiredState(desiredStates[3]);
+    }
 
-  /**
+    /** Resets the drive encoders to currently read a position of 0. */
-   * Returns the turn rate of the robot.
+    public void resetEncoders() {
-   *
+        m_frontLeft.resetEncoders();
-   * @return The turn rate of the robot, in degrees per second
+        m_rearLeft.resetEncoders();
-   */
+        m_frontRight.resetEncoders();
-  public double getTurnRate() {
+        m_rearRight.resetEncoders();
-    return m_gyro.getRate(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
+    }
-  }
+
+    /** Zeroes the heading of the robot. */
+    public void zeroHeading() {
+        m_gyro.reset();
+    }
+
+    /**
+     * Returns the heading of the robot.
+     *
+     * @return the robot's heading in degrees, from -180 to 180
+     */
+    public double getHeading() {
+        return Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)).getDegrees();
+    }
+
+    /**
+     * Returns the turn rate of the robot.
+     *
+     * @return The turn rate of the robot, in degrees per second
+     */
+    public double getTurnRate() {
+        return m_gyro.getRate(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
+    }
 }

src/main/java/frc/robot/subsystems/MAXSwerveModule.java

@@ -17,7 +17,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType;
 import com.revrobotics.AbsoluteEncoder;
 import com.revrobotics.RelativeEncoder;
 
-import frc.robot.Configs;
+import frc.robot.constants.ModuleConstants;
 
 public class MAXSwerveModule {
   private final SparkMax m_drivingSpark;
@@ -51,9 +51,9 @@ public class MAXSwerveModule {
     // Apply the respective configurations to the SPARKS. Reset parameters before
     // applying the configuration to bring the SPARK to a known good state. Persist
     // the settings to the SPARK to avoid losing them on a power cycle.
-    m_drivingSpark.configure(Configs.MAXSwerveModule.drivingConfig, ResetMode.kResetSafeParameters,
+    m_drivingSpark.configure(ModuleConstants.drivingConfig, ResetMode.kResetSafeParameters,
         PersistMode.kPersistParameters);
-    m_turningSpark.configure(Configs.MAXSwerveModule.turningConfig, ResetMode.kResetSafeParameters,
+    m_turningSpark.configure(ModuleConstants.turningConfig, ResetMode.kResetSafeParameters,
         PersistMode.kPersistParameters);
 
     m_chassisAngularOffset = chassisAngularOffset;