Hey, sorry to take so long to get back to you. I did manage to glance through your code and nothing leapt out at me. I can tell you this, though, we’ve also been struggling with the Democat library and last night we took the plunge and switched over to YAGSL. Within a few hours we had a working swerve. Had to make sure the motor and gyro inversions were correct and if you have your robot on blocks, their driving code doesn’t like it, but it was so simple to get up and running.
This is the key file here.
https://github.com/FRC5010/FRCLibrary/blob/main/FRC5010Example2023/src/main/java/frc/robot/FRC5010/drive/swerve/YAGSLSwerveDrivetrain.java
You can find YAGSL here: