Path Following

A very common thing you might want to do is actually follow the generated path. Oxplorer outputs both standard WPILib trajectories and a custom path structure, which provides a lot of flexibility in your path following.

Example Command for Swerve

This command is a modified version of WPILib's SwerveControllerCommand. You could use theirs if you prefer, but it will require some additional setup. This command has been used on a real robot (albeit at very slow speeds), and functions as expected.

public class FollowTrajectory extends Command {
    private final Timer m_timer = new Timer();
    private final Trajectory m_trajectory;
    private final HolonomicDriveController m_controller;
    private final Supplier<Rotation2d> m_desiredRotation;
    private final DriveSubsystem m_driveSubsystem;

    /**
     * Constructs a new FollowTrajectory command that when executed will follow the
     * provided trajectory. This command will not return output voltages but rather
     * raw module states from the position controllers which need to be put into a
     * velocity PID.
     *
     * <p>
     * Note: The controllers will *not* set the outputVolts to zero upon completion
     * of the path- this is left to the user, since it is not appropriate for paths 
     * with nonstationary endstates.
     *
     * @param trajectory      The trajectory to follow.
     * @param controller      The HolonomicDriveController for the drivetrain.
     * @param desiredRotation The angle that the drivetrain should be facing.
     *                        This is sampled at each
     *                        time step.
     * @param driveSubsystem  The subsystem to use to drive the robot.
     * @param requirements    The subsystems to require.
     */
    public FollowTrajectory(
            Trajectory trajectory,
            HolonomicDriveController controller,
            Supplier<Rotation2d> desiredRotation,
            DriveSubsystem driveSubsystem,
            SubsystemBase... requirements) {
        m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
        m_controller = requireNonNullParam(controller, "controller", "SwerveControllerCommand");
        m_driveSubsystem = requireNonNullParam(driveSubsystem, "driveSubsystem", "SwerveControllerCommand");
        m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");

        addRequirements(requirements);
    }

    @Override
    public void initialize() {
        m_timer.restart();
    }

    @Override
    public void execute() {
        double curTime = m_timer.get();
        var desiredState = m_trajectory.sample(curTime);
        Rotation2d desiredRotation = m_desiredRotation.get();
        ChassisSpeeds targetChassisSpeeds = m_controller.calculate(m_driveSubsystem.getPose(), desiredState, desiredRotation);
        // This is done because the rotation is inverted. 
        // It may not be the same on your robot, so if the rotation does not function as expected, remove this.
        targetChassisSpeeds.omegaRadiansPerSecond *= -1;
        var targetModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(targetChassisSpeeds);
        m_driveSubsystem.setModuleStates(targetModuleStates);
    }

    @Override
    public void end(boolean interrupted) {
        m_timer.stop();
    }

    @Override
    public boolean isFinished() {
        return m_timer.hasElapsed(m_trajectory.getTotalTimeSeconds());
    }
}

Alternative path following

If you are not satisfied with the path following abilities of the WPILib Trajectory, you can use our custom path structure instead. Here's an example of this:

try {
  // You can also just use Pose2ds
  Path path = pathfinder.generatePath(new Vertex(1, 1), new Vertex(8, 4));
} catch (ImpossiblePathException e) {
  e.printStackTrace();
}

Then, through the getFullPath() method, you can retrieve an ArrayList of vertices, (or Pose2ds, through the asPose2dList() method). You can then follow the path however you would like, such as pure pursuit or other methods.