Any Resources for Auto Path Following? (Swerve)

Hello All,

In the offseason our programming team has been wanting to make a trajectory follower for autos, however, we can not seem to find any good resources online for a step-by-step simple version. We found this video: FRC 0 to Autonomous: #6 Swerve Drive Auto - YouTube, but our robot simply shakes and does nothing more. Does anyone have any other example projects or step-by-step tutorials for us to follow to build this auto system for swerve? It would be a great help to our team.

We are running mk4s with neos and L1s if that info is needed.


Gibson, Team 85


I would recommend looking at PathPlanner if you haven’t yet. Swerve path following is non-trivial in the best of circumstances however. It will be easier to help you if you post code here.


As apotato stated, pathplanner may be a very good resource for autonomous path following. Some alternative examples include my team (2875)’s open source swerve library GitHub - CshCyberhawks/SwoLib: Team 2875 Cyberhawks swerve-drive and utility library for FRC (Kotlin/Java) which contains some PID-corrected on-the-fly autonomous trajectory generation (warning: it is written in kotlin and uses custom Kinematics and odometry - so if you are using WPILib’s swerve classes it may not be that helpful). Another useful resrouce may also be Mechanical Advantage’s open source code (GitHub - Mechanical-Advantage/RobotCode2023: Robot code for "Banana Split"), as they also have the dynamic auto trajectory generation (their’s is in Java and uses a few more WPILib classes).Unfortunately, I don’t know if any step-by-step tutorials for implementing this. I had started an FRC swerve tutorial series; however, I unfortunately never got up to the autonomous. If I end up finishing it soon, I will update this comment.

The shaking and lack of movement sounds like it may be some poor tuning (on PID or trapezoids?)

I had tried to make PathPlanner easier to use with YAGSL too. Even included it in the example repo!

If you want to look more into the theory behind these things, and understand the control theory fully, I recommend taking a look at this resource:

I started in the WPILib SwerveControllerCommand Example. This example include a basic S curve in Auto. It’s a great starting point.

This example shows the process of generating a trajectory from waypoints and then following that trajectory with a Command created by an instance of a SwerveControllerCommand object.

You need to get your head around this

  frc2::SwerveControllerCommand<4> swerveControllerCommand(
      exampleTrajectory, [this]() { return m_drive.GetPose(); },


      frc2::PIDController{AutoConstants::kPXController, 0, 0},
      frc2::PIDController{AutoConstants::kPYController, 0, 0}, thetaController,

      [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },


The first parameter exampleTrajectory holds the list points along the path. These points contain information like x coordinate, y coordinate, time stamp, velocity, path heading etc.

The next parameter [this]() { return m_drive.GetPose(); } is a lambda function that when called will return the current pose of the robot (a pose is an x, y, theta). This is called repeatedly as the command is executing to check if the robot is on path. You have to have odometry setup working correctly before any of this path following stuff will work.

The next parameter m_drive.kDriveKinematics is used inside the command so that the resultant chassis speed generated by the follower can be converted into swerve module states. Internally the path follower calculates x, y, and theta velocities. (Just like those returned from a joy stick).

The next two parameters frc2::PIDController{AutoConstants::kPXController, 0, 0}, frc2::PIDController{AutoConstants::kPYController, 0, 0},
are instances of PID controllers. In reality they are just proportional controllers as the I and D terms are set to zero. These two controllers manage the feedback corrective action in the X direction and Y direction.

Inside the command the current X and Y coordinates of the robot (found by calling the 2nd paramter ie GetPose) are compared to where the robot should be at this point in time along the path (found through sampling the list of trajectory points.) These errors are multiplied by the Proportional constants. There is also a feedforward component added to these results. The feed forward is calculated by looking at the velocity the robot should be doing at this in time and the direction that it should be heading at this point in time. (Both of these bits of information are stored in the list of trajectory points.) A bit of trig and you have the x velocity and y velocity that the robot should be doing to follow the path at this point. If you have your swerve modules setup well (ie when you ask a swerve module to do 2 metre per second it actually does 2 metres per second) the feed forward should do 99% of the work of keeping the robot on the correct path. The feedback is just there to add a little bit more or take a bit away if the robot isn’t where it should be.

The next parameter thetaController is a profiled PID controller that created beforehand and passed in. The profiled bit means that the output of the PID controller is kept under control. (ie Cant rapidly jump from one value to another, it can only change its output value at the acceleration and max velocity rate defined when the profiled controller is created). The reason this one is a profiled controller and the others aren’t is because there is no need to profile the X and Y coordinates as trajectory points are already supplied in a profiled form by the trajectory generator).
But by using another constructor to the SwerveControllerCommand you can also supply another parameter that when called will return what holonomic rotation the robot should be facing. Because this is not created by WPILib trajectory generator it could be any value. Lets say you want the robot to perform a 180 degree spin as it follows the path. You can supply the SwerveControllerCommand object with a desired finishing holonomic heading right at the start and the SwerveControllerCommand with supply appropriate theta values to spin the robot smoothly as the robot goes along the path. If you want custom control over the holonomic rotation I suggest moving onto pathplanner, bt its best to get your head around WPILibs trajectory following before moving on to more advanced stuff.

The next parameter [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); is another lambda function, but this time its a function that consumes values instead of supplying them. Every time the SwerveControllerCommand object calculates the x, y and theta velocities needed to follow the path it puts them through the kinematics to convert them to module states and then gives these module states to the drive subsystem to execute them.

The last parameter {&m_drive} is the subsystem that is used by the command. Internally it requires this subsystem so that it has full control over it This basically disconnects the joystick while following the trajectory, because the joystick is connected to the subsystem through the default command.

The <4> is the number of swerve modules in your kinematics object. If you have a wheel swerve robot with 2 casters as the other wheels, this would be <2>.

I found it helpful to look at the sources code inside the SwerveControllerCommand to really understand what was going on.


If you can understand all of that, you are well on your way to mastering SwerveDrive Path Following.

Next steps from here is to control the holonomic rotation as you travel the path. I would then try and get your head around pathplanner.

This is next level stuff in here. This has the ability to build a single command group that will not only follow a path with controlled holonomic rotation but also fire off commands at waypoints, and points along the path, as sequential or parallel or deadline groups groups with wait time and the whole nine yards. Basically creates a single command that will do your entire auto routine controlling every subsystem.

This is not for the faint hearted. You definitely need to under the basic Swerve Path Following (From WPILib) before you try and get your head around this stuff. But once you do, your autos will go to the next level.

Happy to answer answer questions if you have any.



I just watched that video you linked. I think that is really great resource. I wished I watched that before attempted to explain everything in text, I think he does a much better job then me. If you are reading this and programming in Java and haven’t watched the video, definitely go and watch it now.

Tip: The shaking can happen if your joystick still has control of your drive subsystem while trying to follow a path in auto. What’s happening is the path following is creating ChassisSpeeds to follow, but the joystick is saying do nothing because it is centered. If the shaking gets more violent as time progresses, this is a sure indicator that this is the problem. This is because the positional error is getting bigger and bigger as time goes on and the speeds are repeatably jumping from 0 to max value 50 times a second. An easy way to test this is push the joystick in the rough direction of the desired path while attempting to follow the path in auto, then try and push it in the opposite direction. You will see if your joystick is supplying any input while it should be following the path.

The auto command requiring the drivesubsystem should disconnect the joystick (because the joystick control is usually bound to the subsystems default command), but there might be something funky going on in your code where the auto is not requiring the drive subsystem. We had this exact same problem on our journey to understanding Serve path following, when we starting trying to implement pathplanner.

The other thing that can cause a problem if your are using pathplanner and your are returning chassis speed objects instead of swerve module states is you need to be careful that you don’t rotate them twice in the conversation between robot centric and field centric. We also came across this issue. They were being rotated in the path follower and then again in the drive subsystem.

Code that turns off the steering motors when joysticks inputs are near zero (to stop that module jitter) can also cause grief in auto followers.

Hope this helps.

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