Probier mal, mit größerem Prescale die LCD-Routinen gleich auch in der ISR zu machen.
Nachdem du ihn ohnehin ja viele floats rechnen läßt, kommt's darauf wohl nicht mehr an.
Da ich vermute, daß sich ISR und normal Loop in die Quere kommen, wär es einen Versuch wert.

EDIT: In dieser Kurzversion werden "Soll" und "Ist" gar nicht verändert.
Er müßt da ja immer 0 zeigen.