Tuning out Drift

When trying to calculate your current pose, the pose estimator does an excellent job to a degree (you can tune this but most teams don't).

The very first thing that you should tune is the drive and angle PIDs.

Teams should tune the SwerveDrivePoseEstimator visionStdDevif you’re using vision to help odometry. To tune vision it’s mostly guess and check on the standard deviation with a scale depending on distance. Some teams experience better stability by filtering out all estimated poses X distance away.

After that you can tune discretization to compensate for the system delay. More information on tuning discretization is here.

After that you can tune your angular velocity coefficient to compensate for gyro system delay (not as necessary when you have a pigeon on a canivore).

All of these are individual tuning steps which build on top of each other.

If you mess with a lower step all the higher steps will be out of sync, and if you need to replace a part like your gyro you may need to redo all of it anyways.

The math heavy explanation on tuning out drift is available here.

GitHub - calcmogul/controls-engineering-in-frc: Book source for Controls Engineering in the FIRST Robotics CompetitionGitHub