Soweit sollte es schon passen. Du hast nur die Tasten im Programm auf PIND.