Software fixes to mechanical problems... eek!
If your robot is drifting... there's one quick thing that you should look at right away... this is a very typical situation, but it can often (but not always) be adjusted by shifting your weight distribution.
The drill motors do not run in the same speed forward and backward, if this is what you're using, (or even if you're not) perhaps you could shift some weight around your robot to the side opposite of where you're drifting... i.e. if you're drifting to the right, try to move some weight on the left side.
Mind you that in reverse, drifting will be more significant if you're evenly balanced via weight in forward. Maybe you'll balance this by software in reverse, since you can try to avoid needing to drive accurate and pushing in that direction.
In general, try to do as much as you can mechanically, and don't try to kill your robot's power input through software... your drivers wil want every ounce of it!
Good luck!
Matt