We need to be able to re-sync the steering encoders on the fly in case something goes wrong.
We already have code to do this but we have to be careful to not mess up anything else.
Tasks:
[ ] Create a command to SyncSteerOnTheFly. This should have code in initlize() and then immedialty return true to isFinished()
[ ] Call the resetEncodersFromCanCoder method in SwerveSubsystem
[ ] After resetEncodersFromCanCoder, call resetLastAngles method in SwerveSubsystem. This will make sure the lastAngle variables are updated properly after syncing.
[ ] Store the current odometry value before resetting the encoders. Then after resetting, call resetOdometry to put back this value.
[ ] Bind to a button on driver controller. This should probably go on the start button and the cone driver aid that's there should be moved to another button. Start button is good because it's hard to accidently press (which might mess things up if used at the wrong time)
We need to be able to re-sync the steering encoders on the fly in case something goes wrong.
We already have code to do this but we have to be careful to not mess up anything else.
Tasks: