Created basic swerve subsystem skeleton

src/main/java/org/frc6423/frc2025/Constants.java

@@ -12,6 +12,8 @@
 /** A class */
 public class Constants {
 
+  public static final double kTickSpeed = 0.02;
+
   // * Select which robot to initalize
   private static final RobotType m_robotType = RobotType.SIMULATED;
 

src/main/java/org/frc6423/frc2025/subsystems/swerve/SwerveSubsystem.java

@@ -0,0 +1,113 @@
+package org.frc6423.frc2025.subsystems.swerve;
+
+import static org.frc6423.frc2025.Constants.kTickSpeed;
+
+import java.util.Arrays;
+
+import org.frc6423.frc2025.subsystems.swerve.module.Module;
+import org.frc6423.frc2025.util.swerveUtil.SwerveConfig;
+
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class SwerveSubsystem extends SubsystemBase {
+    private final SwerveConfig m_config;
+
+    private final Module[] m_modules;
+
+    private SwerveDriveKinematics m_kinematics;
+    private SwerveDrivePoseEstimator m_poseEstimator;
+
+    public SwerveSubsystem(SwerveConfig config) {
+        // Create modules
+        var moduleConfigs = config.getModuleConfigs();
+        m_modules = new Module[moduleConfigs.length];
+        Arrays.stream(moduleConfigs)
+            .forEach((c) -> m_modules[c.kIndex] = new Module(c));
+
+        // Create math objects
+        m_kinematics = new SwerveDriveKinematics(config.getModuleLocs());
+        m_poseEstimator = new SwerveDrivePoseEstimator(m_kinematics, getHeading(), getModulePoses(), new Pose2d());
+
+        m_config = config;
+    }
+
+    @Override
+    public void periodic() {
+
+        // Stop module input when driverstation is disabled
+        if (DriverStation.isDisabled()) {
+            for (Module module : m_modules) {
+                module.stop();
+            }
+        }
+    }
+
+    /**
+     * Runs swerve at desired robot relative velocity
+     * 
+     * @param velocity desired velocity
+     * @param openloopEnabled enable or disable open loop voltage control
+     */
+    public void runVelocities(ChassisSpeeds velocity, boolean openloopEnabled) {
+        velocity = ChassisSpeeds.discretize(velocity, kTickSpeed);
+
+        SwerveModuleState[] desiredStates = m_kinematics.toSwerveModuleStates(velocity);
+        SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, m_config.getMaxLinearSpeedMetersPerSec());
+
+        for(int i = 0; i < desiredStates.length; i++) {
+            if (openloopEnabled) {
+                ChassisSpeeds currentVelocities = getVelocitiesRobotRelative();
+                boolean focEnabled = 
+                    Math.sqrt(
+                        Math.pow(currentVelocities.vxMetersPerSecond, 2) 
+                        + Math.pow(currentVelocities.vyMetersPerSecond, 2)) // converts linear velocity components to linear velocity
+                    < 1 * m_config.getMaxLinearSpeedMetersPerSec();
+
+                    // Converts desired motor velocity into input voltage
+                    // OmegaRadsPerSec/(KvRadsPerVolt)
+                    double driveVoltage = desiredStates[i].speedMetersPerSecond / DCMotor.getKrakenX60(1).KvRadPerSecPerVolt;
+                    m_modules[i].runSetpointOpenloop(driveVoltage, desiredStates[i].angle, focEnabled);
+            } else {
+                m_modules[i].runSetpoint(desiredStates[i]);
+            }
+        }
+    }
+
+    /** Gets current robot velocity (robot relative) */
+    public ChassisSpeeds getVelocitiesRobotRelative() {
+        return m_kinematics.toChassisSpeeds(getModuleStates());
+    }
+
+    /** Gets current robot heading */
+    public Rotation2d getHeading() {
+        return new Rotation2d();
+    }
+
+    /** Gets current robot field pose */
+    public Pose2d getPose() {
+        return m_poseEstimator.getEstimatedPosition();
+    }
+
+    /** Returns an array of module field positions */
+    public SwerveModulePosition[] getModulePoses() {
+        return Arrays.stream(m_modules)
+            .map(Module::getModulePose)
+            .toArray(SwerveModulePosition[]::new);
+    }
+
+    /** Returns an array of module states */
+    public SwerveModuleState[] getModuleStates() {
+        return Arrays.stream(m_modules)
+            .map(Module::getModuleState)
+            .toArray(SwerveModuleState[]::new);
+    }
+}

src/main/java/org/frc6423/frc2025/subsystems/swerve/constants/CompBotSwerveConfigs.java

@@ -1,5 +1,7 @@
 package org.frc6423.frc2025.subsystems.swerve.constants;
 
+import java.util.Arrays;
+
 import org.frc6423.frc2025.util.swerveUtil.ModuleConfig;
 import org.frc6423.frc2025.util.swerveUtil.SwerveConfig;
 
@@ -94,7 +96,7 @@ public double getWheelRadiusInches() {
 
     @Override
     public ModuleConfig[] getModuleConfigs() {
-        return new ModuleConfig[] {
+        var configs = new ModuleConfig[] {
             new ModuleConfig(
                 1, 
                 1, 
@@ -132,6 +134,11 @@ public ModuleConfig[] getModuleConfigs() {
                 getPivotConfigSparkMax(), 
                 getDriveConfigSparkMax()),
         };
+
+        Arrays.stream(configs)
+            .forEach((c) -> c.kWheelRadiusMeters = Units.inchesToMeters(getWheelRadiusInches())); // Shut up
+
+        return configs;
     }
 
     // Gains

src/main/java/org/frc6423/frc2025/subsystems/swerve/module/Module.java

@@ -5,7 +5,9 @@
 
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
 
 public class Module {
     private final ModuleIO m_IO;
@@ -35,13 +37,13 @@ public void updateInputs() {
 
     /** Periodically ran logic */
     public void periodic() {
-        Logger.processInputs("Swerve/Module" + m_index, m_inputs);
+        Logger.processInputs("Swerve/Module" + getModuleIndex(), m_inputs);
     }
 
     /** Run SwerveModuleState setpoint */
     public SwerveModuleState runSetpoint(SwerveModuleState setpointState) {
         setpointState.optimize(getPivotAngle());
-        setpointState.speedMetersPerSecond *= setpointState.angle.minus(getPivotAngle()).getCos();
+        setpointState.speedMetersPerSecond *= Math.cos(setpointState.angle.minus(getPivotAngle()).getRadians());
 
         double speedMPS = setpointState.speedMetersPerSecond;
         m_IO.setPivotAngle(setpointState.angle);
@@ -56,9 +58,11 @@ public SwerveModuleState runSetpoint(SwerveModuleState setpointState, double dri
     }
 
     /** Runs SwerveModuleState setpoint but runs drive in open loop mode */
-    public SwerveModuleState runSetpointOpenloop(SwerveModuleState setpointState, boolean FOCEnabled) {
+    public SwerveModuleState runSetpointOpenloop(double voltage, Rotation2d angle, boolean FOCEnabled) {
+        SwerveModuleState setpointState = new SwerveModuleState(voltage, angle);
+
         setpointState.optimize(getPivotAngle());
-        setpointState.speedMetersPerSecond *= setpointState.angle.minus(getPivotAngle()).getCos();
+        setpointState.speedMetersPerSecond *= Math.cos(setpointState.angle.minus(getPivotAngle()).getRadians());
 
         double speedMPS = setpointState.speedMetersPerSecond;
         m_IO.setPivotAngle(setpointState.angle);
@@ -92,8 +96,33 @@ public int getModuleIndex() {
         return m_index;
     }
 
+    /** Get Module config */
+    public ModuleConfig getModuleConfig() {
+        return m_config;
+    }
+
+    /** Gets Drive Speed in MPS */
+    public double getVelMetersPerSec() {
+        return m_inputs.driveVelRadsPerSec * m_config.kWheelRadiusMeters;
+    }
+
     /** returns current module angle */
     public Rotation2d getPivotAngle() {
         return m_inputs.pivotABSPose;
     }
+
+    /** Returns drive pose in meters */
+    public double getPoseMeters() {
+        return m_inputs.drivePoseRads * m_config.kWheelRadiusMeters; 
+    }
+
+    /** Returns swerve field pose */
+    public SwerveModulePosition getModulePose() {
+        return new SwerveModulePosition(getPoseMeters(), getPivotAngle());
+    }
+
+    /** Returns Module state */
+    public SwerveModuleState getModuleState() {
+        return new SwerveModuleState(getVelMetersPerSec(), getPivotAngle());
+    }
 }

src/main/java/org/frc6423/frc2025/util/swerveUtil/ModuleConfig.java

@@ -12,6 +12,7 @@
 import com.revrobotics.spark.config.SparkMaxConfig;
 
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
 
 public class ModuleConfig {
   public int kIndex;
@@ -29,6 +30,8 @@ public class ModuleConfig {
 
   public SparkMaxConfig kPivotConfigSparkMax, kDriveConfigSparkMax;
 
+  public double kWheelRadiusMeters = Units.inchesToMeters(2);
+
   /** Create a new TalonFX modul */
   public ModuleConfig(
       int index,