How did you determine your P, I, and D gains? If you have not tuned your controller, you'll get poor performance. Running with execution highlighting will slow down the execution, which will change the effect of the gains. I would start with pure proportional control (I and D = 0) with a small proportional gain, and see if it responds in approximately the way you want. If so, then your code is probably correct, and you just need to do a full tuning to get good performance. There are many resources available on the web, on this forum and elsewhere, for PID tuning. Note that since your code uses a simple PID example, and not the one from the PID toolkit, the gains do not have the units that are used in many tutorials so you'll have to correct the calculations to compensate.
