This code was written for team 5588T, but it can be easily modified to work with your team's robot.
This code is being thoroughly rewritten for the 2022-2023 season (Spin Up) to use odometry over controller inputs. I highly recommend against using the current version of this code, as it does not work as well as it could.