Working C++ Swerve Drive - Super Proud!

Hello, Chief Delphi :slightly_smiling_face:
My repository
Plus, a video of this thing running (capped at 20% max velocity)

I just wanted to share with everyone a last-minute solo project that I put quite a bit of time into the final ~30 days of my senior year. I got a 30″x30″ frame with SDS Mk. IV modules and Falcon 500 motors zipping around using C++. The team I was a part of had never tackled programming swerve drive, and I know that the vendor publishes code that is more than sufficient (shoutout to SDS for quickly becoming a respected name in FRC, and well deserved), but I wanted to actually do the work myself. I also wanted to dip my toes into C++.

Since I noticed a bit of a deficit in the number of C++ examples while coding this project, I wanted to take the time to put this out there. The code is by no means perfect: PID for drive is zeroed out since it varies from robot to robot, and at the time of development, I did not have a final weight; there are a few instances where the units library should be extended further (CANCoder native-units comes to mind); sample autonomous is not provided. All of that said, I don’t believe that there are any bad practices displayed, and I don’t want to break functionality by making these minor changes without being able to retest with hardware. The repository, as it sits, is complete.

The PID is done using the TalonFX controllers, and the math leverages WPI kinematics and module states (with the exception of Optimize(), which I had to rewrite to accomodate CTRE’s non-continuous PID controllers). The result is a project with similar functionality to the WPILib example swerve repository.

If anyone has any feedback, I’d greatly appreciate it. I’m very pleased to have gotten this finished!


Thank you so much for this!

1 Like

That C++ project is pretty clean overall. My main suggestions would be:

Use fmtlib (based on C++20’s std::format()) instead of std::cout for prints. It’s easier to use and more efficient. WPILib ships with the library already.

Passing double[] for module parameters is incorrect because motor IDs have type int. Also, passing variables for unrelated things in this format makes the purpose of each element unclear. Instead, pass each variable as a separate constructor argument and document them in a Doxygen comment.

Default-initialize your member variables in the header when possible (e.g., initialize desired angle with zero).

Add Doxygen comments to all your functions that describe what they do and include an @param tag for each argument. For example:

 * Does something.
 * @param turnMotorID Turn motor CAN ID.
 * @param encoderID Encoder CAN ID.
void func(int turnMotorID, int encoderID);
1 Like

Thanks, Tyler! I appreciate you taking the time to look it over. Most of these were on my to-do list prior to the end of the season; now that I am in college, I don’t have quite as much time, but I will definitely integrate these suggestions into the main repository. I want it to be a good resource for future teams looking for C++ examples.


This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.