Ist das ein Beispiel aus dem Buch? Wenn nicht würde ich das Programm so umschreiben das du den Timer/Counter direkt zur PWM erzeugung nutzt (Phase Correct PWM oder Fast PWM).

MfG Hannes