Look at the servo signal with an oscilloscope and the servo removed. Is this a stable signal? If not the the constant looking at the IR is interfering with the software generation of the servo signal. Switch to using a hardware generated servo signal.