Last one for now. The set method in SwerveDriveWheel.java is confusing and potentially wrong. Here's what I think is trying to happen from reading through it (please correct me if I'm wrong both @Z-ach and I are confused as to what exactly is going on).

First we note that asking the wheel to be at 70 degrees and spinning at 100rpm is the same as being at 250 degrees while spinning at -100rpm. So if we are at 240 degrees and want to be at 70 degrees it is better to pivot the 10 degrees to get to 250 instead of the 190 degrees to get to 70. This is what closestAngle is trying to do, given the desired angle and the current angle tell us which "mode" we're in.

Based on the mode that closestAngle determines we then have to modify our speed, our setpoint, and our output angle accordingly. That's what the nest of if's and elses are trying to do, determine how to modify everything.

What's confusing me is how this nest of if's and elses makes these decisions. The way I see it those series of steps are equally well encoded with this block of python:

def normalize_angle(angle):
    """Ensure that angles exist in the range of -180 to 180"""
    angle = angle % 360
    if angle > 180:
        angle -= 360
    return angle


def set(setpoint, currentAngle, speed):
    error = normalize_angle(setpoint - currentAngle)
    potential_setpoint = (setpoint + 180) % 360
    potential_error = normalize_angle(potential_setpoint - currentAngle)

    if abs(potential_error) < abs(error):
        setpoint = potential_setpoint
        speed = -speed
        error = potential_error

    # Not sure what this is, it's odd that it's not 0 if the error is 0
    output = (kP * abs(error)) + (
        (0.4 / (1 + 3 * (math.exp(-0.01 * abs(error))))) - 0.06
    )

    if error < 0:
        output = -output

    return output, -speed

However this implementation and the one with the modes don't always agree, in particular if the setpoint is -153 and the current angle is 350 the current implementation wants the output to be 143 while the above implementation wants 37. If setpoint and current angle are both in the range 0 to 360 these two implementations seem to agree.

Currently there is a lot of repetition in the constructor for the drivetrain subsystem. In particular this chunk seems to just be doing the same thing 4 times in a row. It seems you could define a method like so:

  private static initSwerveModule(double P, double I, double D, int direction_drive, int rotation_drive, int coder_drive ) {
    TalonFX directionMotor = new TalonFX(direction_drive);
    CANCoder directionEncoder = m_frontLeftDirectionMotor.getSensorCollection();
    TalonFX rotationMotor = new TalonFX(rotation_drive);
    CANCoder coder = new CANCoder(coder_drive);
    return new SwerveDriveWheel(P, I, D, rotationMotor, coder, directionMotor, directionEncoder);
  }

And then call this function 4 times. This way if you have to ever modify the initialization of the swerve modules you can change it in one place instead of 4 places. This substantially shortens the method:

  public DriveTrain() {

    //swerve wheel PID values
    wkP = 0.005;
    wkI = 0.000001;
    wkD = 0;

    //swerve modules

    m_frontLeftWheel = initSwerveModule(wkP, wkI, wkD, PortConstants.FRONT_LEFT_DIRECTION_DRIVE, PortConstants.FRONT_LEFT_ROTATION_DRIVE, PortConstants.FRONT_LEFT_CODER_DRIVE);
    m_frontRightWheel = initSwerveModule(wkP, wkI, wkD, PortConstants.FRONT_RIGHT_DIRECTION_DRIVE, PortConstants.FRONT_RIGHT_ROTATION_DRIVE, PortConstants.FRONT_RIGHT_CODER_DRIVE);
    m_backLeftWheel = initSwerveModule(wkP, wkI, wkD, PortConstants.BACK_LEFT_DIRECTION_DRIVE, PortConstants.BACK_LEFT_ROTATION_DRIVE, PortConstants.BACK_LEFT_CODER_DRIVE);
    m_backRightWheel = initSwerveModule(wkP, wkI, wkD, PortConstants.BACK_RIGHT_DIRECTION_DRIVE, PortConstants.BACK_RIGHT_ROTATION_DRIVE, PortConstants.BACK_RIGHT_CODER_DRIVE);

    m_gyro = new AHRS(Port.kMXP);

    m_gyro.zeroYaw();
    m_controller = new SwerveDriveController(m_frontLeftWheel, m_frontRightWheel, m_backLeftWheel, m_backRightWheel, m_gyro);
  }

(Side note y'all don't seem to ever use the RotationEncoders you initialize)

The github repository should probably be updated to use master as the default branch as opposed to main as that seems to be the main branch y'all are using. Alternatively you could merge all the stuff into main. As it stands right now when you clone the repository you get an empty project.

There is a pattern I'm seeing all over the place to try and normalize angles. It looks something like this:

error = Math.abs(setpoint - currentAngle);
if (error > 360) {
    error -= 360;
}

This works so long as the error is never larger than 720, I suspect this is generally the case but the slightly more correct version would be to use a loop:

error = Math.abs(setpoint - currentAngle);
while (error > 360) {
    error -= 360;
}

Or just take the modulo of the error:

error = Math.abs(setpoint - currentAngle) % 360;

Peeking at the code y'all have written it looks like you're trying to do a singleton design pattern with the DriveTrain subsystem or at least I assume that's what these lines are here for. However you have made your constructor public so there is actually nothing enforcing the singleton nature of the drive train.