PID Controller

I have been working on the PID controller element of the software as I now have an object oriented program layed out.
To test various elements of the PID controller I have had to first use a disk dump to dump all of the figures produced to file for examination then as I narrowed down the PID settings I used the InputBox method so i could enter figures manually without having to recompile the program every time.
At the moment I am only using Proportional control with a gain of 2 which gives me an almost straight line and any other setting result in massive overshoot. The robot is only sampling PID 3 times per second so I may need to optimize the code a little further. Also the wheels do not seem to send enough data so I might have to drill a few extra holes to get more pulses per rotation.