für "warte()" gibt es in der util/delay.h bereits eine funktion:

Deine Funktion ist nicht von der F_CPU abhängig, also eigentlich unbrauchbar.
Zu deiner eigentlichen Frage bräuchte man noch etwas mehr Informationen, ein Schaltplan wäre einmal ein Anfang.
Deinen Code könntest du auch etwas erklären, also was er im Moment kann, was er nicht kann und was er können sollte.
Vorallem so etwas ist irgendwie... verwirrend, um es mal milde auszudrücken.
Code:
#define B 0
#define D 1