Das Programm scheint richtig zu sein. Bedenke nur das while() auch ein paar Take an Zeit braucht.
Wenn die Schleifen nicht duch ein Interrupt unterbrochen werden, muss was mit dem ext. Quarz oder den FuseBits nicht stimmen.