I started in the WPILib SwerveControllerCommand Example. This example include a basic S curve in Auto. It’s a great starting point.
https://github.com/wpilibsuite/allwpilib/tree/main/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand
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(); },
m_drive.kDriveKinematics,
frc2::PIDController{AutoConstants::kPXController, 0, 0},
frc2::PIDController{AutoConstants::kPYController, 0, 0}, thetaController,
[this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },
{&m_drive});
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.
https://github.com/wpilibsuite/allwpilib/blob/main/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
and
https://github.com/wpilibsuite/allwpilib/blob/main/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
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.
https://github.com/mjansen4857/pathplanner
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.
Cheers
Warren
6508