[Oblarg]

Things to watch out for when doing this, based on personal experience:

1) It is very hard for a loop to keep you driving straight at max speed, because if your motors are saturated you can only slow down one side to adjust, rather than both slow down one side and speed up the other. If you give yourself some headroom, it will work much better.

2) Unless you are using cascading control (i.e. feeding the output of your heading loop to a wheel velocity loop), it is likely that you won't be able to make the p gain high enough on the heading loop to make steady state error acceptably small without introducing instability. A quick and dirty fix for this is to simply add or subtract some minimum output to/from the calculated heading loop output whenever it is outside of some tolerance around 0.