[AustinSchuh]

One way to help solve the problem is to implement Feed Forward. The basic idea behind Feed Foward is to utilize any extra information you know to make the controller more accurate and responsive.

For driving, the best feed forward information to utilize is your desired drive velocity. If you want to drive at 1/2 of full speed, it takes a certain power level to be applied to each wheel. That number won't change too much, or at least more than your PID controller can't deal with. You can implement that term by adding in a power value to the PID controller that you look up in a lookup table using the goal velocity.

You can have as much fun as you want imagining all sorts of feed forward ideas to implement that will help model what power you need to give the motors before the PID controller deals with the last little bit. I have always wanted to figure out how to get the controller to add in extra power when it knows it wants to accelerate, but I haven't figured out how to model that yet to implement it.