Create independent settings for each swerve module

1458TeamCode/src/main/java/frc/robot/Robot.java

@@ -15,9 +15,6 @@
  * project.
  */
 public class Robot extends TimedRobot {
-  public static final CTREConfigs ctreConfigs = new CTREConfigs();
-
-
   private Command m_autonomousCommand;
 
   private RobotContainer m_robotContainer;

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

@@ -9,7 +9,7 @@
 
 import frc.robot.autos.*;
 import frc.robot.commands.*;
-import frc.robot.subsystems.*;
+import frc.robot.subsystems.SwerveDrive.Drive;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a

1458TeamCode/src/main/java/frc/robot/autos/Auto.java

@@ -1,7 +1,7 @@
 package frc.robot.autos;
 
 import frc.robot.Constants;
-import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.SwerveDrive.Drive;
 
 import java.util.List;
 

1458TeamCode/src/main/java/frc/robot/commands/TeleopSwerve.java

@@ -1,7 +1,7 @@
 package frc.robot.commands;
 
 import frc.robot.Constants;
-import frc.robot.subsystems.Drive;
+import frc.robot.subsystems.SwerveDrive.Drive;
 
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;

1458TeamCode/src/main/java/frc/robot/subsystems/Drive.java → 1458TeamCode/src/main/java/frc/robot/subsystems/SwerveDrive/Drive.java

@@ -1,4 +1,4 @@
-package frc.robot.subsystems;
+package frc.robot.subsystems.SwerveDrive;
 
 
 import frc.robot.Constants;
@@ -20,22 +20,21 @@
 
 public class Drive extends SubsystemBase {
     public SwerveDriveOdometry swerveOdometry;
-    public Module[] mSwerveMods;
+    public SwerveModule[] mSwerveMods;
     public Pigeon2 gyro;
 
     public Drive() {
         gyro = new Pigeon2(Constants.Swerve.pigeonID, "CV");
         gyro.getConfigurator().apply(new Pigeon2Configuration());
         gyro.setYaw(0);
 
-        mSwerveMods = new Module[]{
-                new Module(0, Constants.Swerve.FrontLeftMod.constants),
-                new Module(1, Constants.Swerve.FrontRightMod.constants),
-                new Module(2, Constants.Swerve.BackLeftMod.constants),
-                new Module(3, Constants.Swerve.BackRightMod.constants)
+        mSwerveMods = new SwerveModule[]{
+                new SwerveModule(SwerveModuleConfig.FRONTLEFT),
+                new SwerveModule(SwerveModuleConfig.FRONTRIGHT),
+                new SwerveModule(SwerveModuleConfig.BACKLEFT),
+                new SwerveModule(SwerveModuleConfig.BACKRIGHT)
         };
 
-
         swerveOdometry = new SwerveDriveOdometry(Constants.Swerve.swerveKinematics, getGyroYaw(), getModulePositions());
     }
 
@@ -55,7 +54,7 @@ public void drive(Translation2d translation, double rotation, boolean fieldRelat
                 );
         SwerveDriveKinematics.desaturateWheelSpeeds(swerveModuleStates, Constants.Swerve.maxSpeed);
 
-        for (Module mod : mSwerveMods) {
+        for (SwerveModule mod : mSwerveMods) {
             mod.setDesiredState(swerveModuleStates[mod.moduleNumber], isOpenLoop);
         }
     }
@@ -64,22 +63,22 @@ public void drive(Translation2d translation, double rotation, boolean fieldRelat
     public void setModuleStates(SwerveModuleState[] desiredStates) {
         SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Constants.Swerve.maxSpeed);
 
-        for (Module mod : mSwerveMods) {
+        for (SwerveModule mod : mSwerveMods) {
             mod.setDesiredState(desiredStates[mod.moduleNumber], false);
         }
     }
 
     public SwerveModuleState[] getModuleStates() {
         SwerveModuleState[] states = new SwerveModuleState[4];
-        for (Module mod : mSwerveMods) {
+        for (SwerveModule mod : mSwerveMods) {
             states[mod.moduleNumber] = mod.getState();
         }
         return states;
     }
 
     public SwerveModulePosition[] getModulePositions() {
         SwerveModulePosition[] positions = new SwerveModulePosition[4];
-        for (Module mod : mSwerveMods) {
+        for (SwerveModule mod : mSwerveMods) {
             positions[mod.moduleNumber] = mod.getPosition();
         }
         return positions;
@@ -110,7 +109,7 @@ public Rotation2d getGyroYaw() {
     }
 
     public void resetModulesToAbsolute() {
-        for (Module mod : mSwerveMods) {
+        for (SwerveModule mod : mSwerveMods) {
             mod.resetToAbsolute();
         }
     }
@@ -124,12 +123,12 @@ public void periodic() {
         }
         swerveOdometry.update(getGyroYaw(), getModulePositions());
 
-        for (Module mod : mSwerveMods) {
+        for (SwerveModule mod : mSwerveMods) {
             SmartDashboard.putNumber("Mod " + mod.moduleNumber + " CANcoder", (mod.getCANcoder().getDegrees()+180)%360);
 
 
 //            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue()%22.0)*360/22-180));
-            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.mAngleMotor.getPosition().getValue() * 360) % 360 + 360) % 360); // This is super specific, don't break this pls
+            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Angle", ((mod.getAngleMotorPosition() * 360) % 360 + 360) % 360); // This is super specific, don't break this pls
 //            SmartDashboard.putNumber("Mod " + mod.moduleNumber + " Velocity", mod.getState().speedMetersPerSecond);
         }
     }

1458TeamCode/src/main/java/frc/robot/subsystems/SwerveDrive/SwerveAngleMotorConfig.java

@@ -0,0 +1,56 @@
+package frc.robot.subsystems.SwerveDrive;
+
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+
+public final class SwerveAngleMotorConfig {
+    public final int angleMotorID;
+    public final boolean isInverted;
+    public final Rotation2d angleOffset;
+    public final TalonFXConfiguration angleFXConfig = new TalonFXConfiguration();
+
+    public SwerveAngleMotorConfig(
+        int angleMotorID,
+        InvertedValue angleMotorInvert,
+        NeutralModeValue angleNeutralMode,
+        double angleGearRatio,
+        int angleCurrentLimit,
+        int angleCurrentThreshold,
+        double angleCurrentThresholdTime,
+        boolean angleEnableCurrentLimit,
+        double angleKP,
+        double angleKI,
+        double angleKD,
+        boolean isInverted,
+        double angleOffset
+    ){
+        /** Swerve Angle Motor Configurations */
+        this.angleMotorID = angleMotorID;
+        this.isInverted = isInverted;
+
+        this.angleOffset = Rotation2d.fromRotations((angleOffset/-360));
+
+        /* Motor Inverts and Neutral Mode */
+        angleFXConfig.MotorOutput.Inverted = angleMotorInvert;
+        angleFXConfig.MotorOutput.NeutralMode = angleNeutralMode;
+
+        /* Gear Ratio and Wrapping Config */
+        angleFXConfig.Feedback.SensorToMechanismRatio = angleGearRatio;
+        angleFXConfig.ClosedLoopGeneral.ContinuousWrap = true;
+
+        /* Current Limiting */
+        angleFXConfig.CurrentLimits.SupplyCurrentLimitEnable = angleEnableCurrentLimit;
+        angleFXConfig.CurrentLimits.SupplyCurrentLimit = angleCurrentLimit;
+        angleFXConfig.CurrentLimits.SupplyCurrentThreshold = angleCurrentThreshold;
+        angleFXConfig.CurrentLimits.SupplyTimeThreshold = angleCurrentThresholdTime;
+
+        /* PID Config */
+        angleFXConfig.Slot0.kP = angleKP;
+        angleFXConfig.Slot0.kI = angleKI;
+        angleFXConfig.Slot0.kD = angleKD;
+    }
+}

1458TeamCode/src/main/java/frc/robot/subsystems/SwerveDrive/SwerveCANCoderConfig.java

@@ -0,0 +1,15 @@
+package frc.robot.subsystems.SwerveDrive;
+
+import com.ctre.phoenix6.signals.SensorDirectionValue;
+import com.ctre.phoenix6.configs.CANcoderConfiguration;
+
+public final class SwerveCANCoderConfig {
+    public final int canCoderID;
+    public final CANcoderConfiguration canCoderConfig = new CANcoderConfiguration();
+
+    public SwerveCANCoderConfig(int canCoderID, SensorDirectionValue cancoderInvert){
+        /** Swerve CANCoder Configuration */
+        this.canCoderID = canCoderID;
+        canCoderConfig.MagnetSensor.SensorDirection = cancoderInvert;
+    }
+}

1458TeamCode/src/main/java/frc/robot/subsystems/SwerveDrive/SwerveDriveMotorConfig.java

@@ -0,0 +1,68 @@
+package frc.robot.subsystems.SwerveDrive;
+
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+
+public final class SwerveDriveMotorConfig {
+    public final int driveMotorID;
+    public final boolean isInverted;
+
+    public final double driveKS;
+    public final double driveKV;
+    public final double driveKA;
+    public final TalonFXConfiguration motorFXConfig = new TalonFXConfiguration();
+
+    public SwerveDriveMotorConfig(
+        int driveMotorID,
+        InvertedValue driveMotorInvert,
+        NeutralModeValue driveNeutralMode,
+        double driveGearRatio,
+        int driveCurrentLimit,
+        int driveCurrentThreshold,
+        double driveCurrentThresholdTime,
+        boolean driveEnableCurrentLimit,
+        double driveKP,
+        double driveKI,
+        double driveKD,
+        double driveKS,
+        double driveKV,
+        double driveKA,
+        double openLoopRamp,
+        double closedLoopRamp,
+        boolean isInverted
+    ){
+        /** Swerve Drive Motor Configuration */
+        this.driveMotorID = driveMotorID;
+        this.isInverted = isInverted;
+
+        this.driveKS = driveKS;
+        this.driveKV = driveKV;
+        this.driveKA = driveKA;
+
+        /* Motor Inverts and Neutral Mode */
+        motorFXConfig.MotorOutput.Inverted = driveMotorInvert;
+        motorFXConfig.MotorOutput.NeutralMode = driveNeutralMode;
+
+        /* Gear Ratio Config */
+        motorFXConfig.Feedback.SensorToMechanismRatio = driveGearRatio;
+
+        /* Current Limiting */
+        motorFXConfig.CurrentLimits.SupplyCurrentLimitEnable = driveEnableCurrentLimit;
+        motorFXConfig.CurrentLimits.SupplyCurrentLimit = driveCurrentLimit;
+        motorFXConfig.CurrentLimits.SupplyCurrentThreshold = driveCurrentThreshold;
+        motorFXConfig.CurrentLimits.SupplyTimeThreshold = driveCurrentThresholdTime;
+
+        /* PID Config */
+        motorFXConfig.Slot0.kP = driveKP;
+        motorFXConfig.Slot0.kI = driveKI;
+        motorFXConfig.Slot0.kD = driveKD;
+
+        /* Open and Closed Loop Ramping */
+        motorFXConfig.OpenLoopRamps.DutyCycleOpenLoopRampPeriod = openLoopRamp;
+        motorFXConfig.OpenLoopRamps.VoltageOpenLoopRampPeriod = openLoopRamp;
+
+        motorFXConfig.ClosedLoopRamps.DutyCycleClosedLoopRampPeriod = closedLoopRamp;
+        motorFXConfig.ClosedLoopRamps.VoltageClosedLoopRampPeriod = closedLoopRamp;
+    }
+}