1902's "minimum viable" swerve drive code... You probably won't be able to copy/paste this whole thing into your own project, but the logic, functions, and structure in this file has worked for us to drive in tele-op.

DriveSubsystem.java

package frc.robot.subsystems;

import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel;
import com.revrobotics.ControlType;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.RobotMap;

import java.util.Arrays;

public class DriveSubsystem extends SubsystemBase {

    private CANSparkMax flDrive, flTurn, frDrive, frTurn, blDrive, blTurn, brDrive, brTurn;
    SwerveDriveKinematics m_kinematics;

    public DriveSubsystem() {
        flDrive = new CANSparkMax(RobotMap.can_flDrive, CANSparkMaxLowLevel.MotorType.kBrushless);
        flTurn = new CANSparkMax(RobotMap.can_flTurn, CANSparkMaxLowLevel.MotorType.kBrushless);
        frDrive = new CANSparkMax(RobotMap.can_frDrive, CANSparkMaxLowLevel.MotorType.kBrushless);
        frTurn = new CANSparkMax(RobotMap.can_frTurn, CANSparkMaxLowLevel.MotorType.kBrushless);
        blDrive = new CANSparkMax(RobotMap.can_blDrive, CANSparkMaxLowLevel.MotorType.kBrushless);
        blTurn = new CANSparkMax(RobotMap.can_blTurn, CANSparkMaxLowLevel.MotorType.kBrushless);
        brDrive = new CANSparkMax(RobotMap.can_brDrive, CANSparkMaxLowLevel.MotorType.kBrushless);
        brTurn = new CANSparkMax(RobotMap.can_brTurn, CANSparkMaxLowLevel.MotorType.kBrushless);

        //All Motors
        for (CANSparkMax sparkMax : Arrays.asList(flDrive, flTurn, frDrive, frTurn, blDrive, blTurn, brDrive, brTurn)) {
            sparkMax.clearFaults();
            sparkMax.restoreFactoryDefaults();
        }

        //Drive Motors
        for (CANSparkMax sparkMax : Arrays.asList(flDrive, frDrive, blDrive, brDrive)) {
            sparkMax.setIdleMode(CANSparkMax.IdleMode.kCoast);
        }

        //Turn Motors
        for (CANSparkMax sparkMax : Arrays.asList(flTurn, frTurn, blTurn, brTurn)) {
            sparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake);
            sparkMax.getEncoder();
            sparkMax.getPIDController().setP(0.1);
        }

        //All
        for (CANSparkMax sparkMax : Arrays.asList(flDrive, flTurn, frDrive, frTurn, blDrive, blTurn, brDrive, brTurn)) {
            sparkMax.burnFlash();
        }

        // Locations for the swerve drive modules relative to the robot center.
        Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
        Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
        Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
        Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);

        // Creating my kinematics object using the module locations
        m_kinematics = new SwerveDriveKinematics(
                m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
        );
    }

    //Runs all motors for a few seconds
    public void testDrive() {
        for (CANSparkMax sparkMax : Arrays.asList(flDrive, frDrive, blDrive, brDrive)) {
            try {
                sparkMax.set(0.4);
                Thread.sleep(1000);
                sparkMax.set(0);
            } catch (InterruptedException ignored) {

            }
        }

        for (CANSparkMax sparkMax : Arrays.asList(flTurn, frTurn, blTurn, brTurn)) {
            try {
                sparkMax.set(0.5);
                Thread.sleep(1000);
                sparkMax.set(0);
            } catch (InterruptedException ignored) {

            }
        }
    }
    
    public void setSpeed(double fl, double fr, double bl, double br) {
        flDrive.set(fl);
        frDrive.set(fr);
        blDrive.set(bl);
        brDrive.set(br);
    }

    public void setAngles(double fl, double fr, double bl, double br) {
        flTurn.getPIDController().setReference(degreesToEncoder(fl), ControlType.kPosition);
        frTurn.getPIDController().setReference(degreesToEncoder(fr), ControlType.kPosition);
        blTurn.getPIDController().setReference(degreesToEncoder(bl), ControlType.kPosition);
        brTurn.getPIDController().setReference(degreesToEncoder(br), ControlType.kPosition);
    }

    //This is for the gearing on swerve drive specialties mk 2... Your formula may vary. 
    private double degreesToEncoder(double degrees) {
        return degrees * 12.8 / 360d;
    }

    //Ditto on message from aboce
    private double encoderToDegrees(double degrees) {
        return degrees * 360 / 12.8;
    }

    //Call this one to drive!!! X and Y are horizontal and vertical speeds, yaw is turn/spin speed. 
    public void wpiSwerveDrive(double x, double y, double yaw) {

        //Deadzone
        if (Math.sqrt(x*x + y*y + yaw*yaw) <= 0.1) {
            setSpeed(0, 0, 0, 0);
            return;
        }

        //The following is copy/pasted from WPILib's docs
        ChassisSpeeds speeds = new ChassisSpeeds(x,y,yaw);

        // Convert to module states
        SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);

        // Front left module state
        SwerveModuleState frontLeft = moduleStates[0];

        // Front right module state
        SwerveModuleState frontRight = moduleStates[1];

        // Back left module state
        SwerveModuleState backLeft = moduleStates[2];

        // Back right module state
        SwerveModuleState backRight = moduleStates[3];

        setSpeed(frontLeft.speedMetersPerSecond, frontRight.speedMetersPerSecond, backLeft.speedMetersPerSecond, backRight.speedMetersPerSecond);
        setAngles(frontLeft.angle.getDegrees(), frontRight.angle.getDegrees(), backLeft.angle.getDegrees(), backRight.angle.getDegrees());
    }

    //Set all turn motors to brake mode (used upon enabling)
    public void lockTurn() {
        for (CANSparkMax sparkMax : Arrays.asList(flTurn, frTurn, blTurn, brTurn)) {
            sparkMax.setIdleMode(CANSparkMax.IdleMode.kBrake);
        }
    }

    //Set all turn motors to coast mode (used upon disabling, so modules can be spun by hand)
    public void unlockTurn() {
        for (CANSparkMax sparkMax : Arrays.asList(flTurn, frTurn, blTurn, brTurn)) {
            sparkMax.setIdleMode(CANSparkMax.IdleMode.kCoast);
        }
    }

    public void resetEncoders() {
        for (CANSparkMax sparkMax : Arrays.asList(flTurn, frTurn, blTurn, brTurn)) {
            sparkMax.getEncoder().setPosition(0);
        }
    }

    //Halt all motion
    public void stop() {
        for (CANSparkMax sparkMax : Arrays.asList(flDrive, flTurn, frDrive, frTurn, blDrive, blTurn, brDrive, brTurn)) {
            sparkMax.stopMotor();
        }
    }

    //Get all encoder positions in one easy-to-read string
    public String log() {
        return String.format("FL: %.2f, FR: %.2f, BL: %.2f, BR: %.2f, GYRO: %.2f",
            flTurn.getEncoder().getPosition(),
            frTurn.getEncoder().getPosition(),
            blTurn.getEncoder().getPosition(),
            brTurn.getEncoder().getPosition(),
                0.0f);
    }
}