You could also look at YAGSL (either to use or as a reference) for swerve drive programming. I would definitely recommend learning how it works however.
Hello World!
YAGSL is a Generic Swerve Library (AN ACTUAL LIBRARY!) that aims to simplify the process of creating a comprehensive and complete SwerveDrive’s with best practices from MANY SwerveDrive examples.
The documentation wiki is here .
An example project is here .
Library is here .
Javadoc are here .
Features:
Second Order Kinematics with limited state-space estimation.
Uses closed-loop PID exclusively.
Dashboard for each swerve module.
Support for both Falcon’s and Neo’s without chang…