Where are the poles and zeros in your servo equations? I cannot believe that you need 1000 Hz response for the tach.

If you can tolerate sluggish acceleration, you might get away with decreasing the P and D terms to almost zero, and using a low gain for I.

The following isn't exactly light reading, but it may very well save you a lot of time. <http://www.ee.usyd.edu.au/tutorials...ocus.html>. Also follow the link to the DC motor example.