Du arbeitest mit wait. Besser und genauer ist es, wenn man einen Timer konfiguriert und diesen Timer eine Variable hochzählt. Wenn die Zeit verstrichen ist Variable auf 0 setzen und den Motor drehen lassen. (Stichwort Timerinterrupt). Welchen µC verwendest du?
Code:
//Deine Headerdateien einfügen
#include <util/delay.h>
#include <avr/interrupt.h>


unsigned char sreg_save;
volatile unsigned char i=0;

//Timerinterrupt
ISR(TIMER0_OVF_vect)
{
	sreg_save=SREG;  //Register sichern
	i++;  //Variable erhöhen
	SREG=sreg_save;  //Gesichertes Register wieder zurückschreiben
}
//Hauptschleife
int main(void)
{
    //Ausgang konfigurieren (musst du anpassen)
	DDRB = 0xff;

	PORTB = 0x00;

   //Timer konfigurieren (musst du noch anpassen => Datenblatt lesen)
	TCCR0B |= (1<<CS00) | (1<<CS02);
	TIMSK |= (1<<TOIE0);

   //Interruptfreigabe
	sei();
 
	while(1)
             {
   // Hier kommt dein Programm her
             }

}