Die Frage beantwortet dir dein AVR mit nem Taster dran, ist mir jetzt zu mühsam mein Brain 1.0 anzukurbeln um das zu emulieren Bild  
Versuch's halt, du hast ja Ideen genug und weisst wie's in C auszudrücken ist. Dadurch und durch evtl. Fehlersuche/Analyse lernste allemal mehr.