Research/Write code that will generate and follow trajectories

[celttechie]

Add methods to the SwerveChassis to specify a heading to rotate to.

[celttechie]

Let's talk about this one tonight... I think we'll want to change how we think about accomplishing this and making the robot more "capable" in the meantime. Specifically, I want to discuss utilizing trajectories to do this, and also give us more possibilities for automatically controlling the "pose" of the robot. See documentation here: https://robotpy.readthedocs.io/projects/wpimath/en/stable/wpimath.trajectory.html

[celttechie]

Here is the WPILIB documentation to show how this all goes together: https://docs.wpilib.org/en/stable/docs/software/pathplanning/trajectory-tutorial/trajectory-tutorial-overview.html

[celttechie]

@Joseph3160 I confirmed that you are right, the Ramsete Controller gives ChassisSpeeds objects with a 0 Y component and only really works for tank drives that can't move sideways. There is this: https://robotpy.readthedocs.io/projects/commands-v2/en/stable/commands2/Swerve4ControllerCommand.html#commands2.Swerve4ControllerCommand, but I'm not sure how it'll work with our closed-loop PID swerve modules. It might be something to try, though.

[celttechie]

This is what we'll want to use instead of a Ramsete Controller: https://robotpy.readthedocs.io/projects/wpimath/en/stable/wpimath.controller/HolonomicDriveController.html

The steps are:

1. Create a TrajectoryConfig
2. Generate a trajectory
3. Create a swerve controller with HolonomicDriveController

Then we need a way to execute the following in a loop:

1. Get a sample from the trajectory sample() method.
2. Pass the sample to the swerve controller calculate() method
3. Give the resulting ChassisSpeeds to the Swerve Chassis

A basic example is here, where the setup is done in robotInit and the trajectory is followed in the AutonomousPeriodic loop: https://github.com/LiamCrafton/liam-wpilib-swerve/blob/d0a7e19e8f596590d3749df1bee3dd9a77546988/robot.py