Trajectory Following for Swerve Problem

Hi all,
I am trying to program autonomous trajectory following with a swerve drive base.
We have swerve working however, it does not use wpi’s SwerveBot example code or command base as the structure. (based off this: Paper: 4 wheel independent drive & independent steering ("swerve"))
To get the autonomous trajectory following working, I’m trying to integrate the HolonomicDriveController. I followed WPI’s documentation and the api but, unfortunately, everything seems to be using units which is a huge pain to convert from ex double.

Is there an easier way to use the Holonomic Drive Controller? Or would it be easier to just create a custom trajectory following program?
Any help is appreciated. Thanks!

1 Like

Are there specific parts of HolonomicDriveController you’ve found cumbersome? It sounds like you’re trying to work against the units system rather than letting it work for you.

Here’s a primer on WPILib’s units system, if you haven’t read it: The C++ Units Library — FIRST Robotics Competition documentation

WPILib C++ adopted its units system in 2020 because it provides automatic conversions between unit types, enforces relationships between different units (e.g., position / time gives velocity, unit conversions happen automatically), and makes APIs more expressive.

Opting out of the units system is sort of a case of “doctor, it hurts when I do this” “well, don’t do that”. The units system’s escape hatches to double like .value() tend to make things verbose, cumbersome, and error-prone since the guarantees of the units system are no longer provided. It’s better to go all-in on the type system instead of trying to work against it.

We’ve wanted to add a units system to Java as well since the expressive power is worth it, and units mistakes are some of the most common errors FRC students make. The problem is Java objects have way too much overhead on top of native doubles. Maybe once a future version of Java gets value classes, we can revisit this.


The C++ units library is amazing and while it might seem imposing at first, once you learn the literal syntax it becomes extremely easy. Keep at it.

I would recommend avoiding HolonomicDriveController for now, though, since it has very confusing semantics as an artifact of the trajectory library originally being written for non-holonomic drives. It should be improved for 2023.

1 Like

I’ve managed to create a swerve point to point trajectory following. I just used pid controllers for forward, strafe, and rotation. It doesn’t account for velocity at points which I will probably implement next.

Here’s a video of it working:


Violently fast, nice!

1 Like