Greetings, saw your post about command based programming. While I can’t comment on vision, the drivetrain aspect looks good.
As far as PID control there are many options for you to take to achieve your objective. What do you want your robot to do autonomously? Drive to a distance? drive straight to a distance?
there are lots of options.
Here are some examples of PID drive control we used for our autonomous is 2015.
recycle rush example PID command
if you look we are actually more concerned with driving straight than exactly how far. there are a few other PID drive commands in their for you to look at.
here is a video of the total auto
Also, during the past season the team has moved to using talon srx’s for all of our PID control. It is much easier once you have the setup done. I would recommend it but they are a bit expensive.
Also bear in mind this is just one way to solve a problem, there are probably better and other ways out there,
Good luck!