WPILib has swerve kinematics, odometry, trajectory generation, and following built in this year. I would check out some examples and the documentation starting at https://docs.wpilib.org/en/latest/docs/software/kinematics-and-odometry/intro-and-chassis-speeds.html
(Ignore differential drive as it’s not for you).
Here’s an example project to drive swerve modules https://github.com/wpilibsuite/allwpilib/tree/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot
Here’s an example project that follows a trajectory.
With everything added to WPILib this year all you’ll need to do is change a couple of inputs and test.
While everything is written for you already, it is highly recommended to understand what exactly is going on under the hood so that you can debug during competition quickly if necessary.