SIGNAL(SIG_OVERFLOW0) { //alle 1ms aufgerufen
TCNT0 = 131; //Zaehler wieder vorladen
if (count < (WIEDERHOLRATE-1)) { //sofort zurueck
count++;
return;
}
if (count == (WIEDERHOLRATE-1)) { //Servogrundpuls 1ms
PORTD |= 0xec; //Pins D2-7 an, 0,1,4 lassen
count++;
return;
}
if (count == WIEDERHOLRATE) { //Servopulse 0...1ms
for (i = 0; i <= 254; i++) {
if (servo1 == i) //Servopin LOW
PORTD &= ~( 1<<PD2 );
if (servo2 == i)
PORTD &= ~( 1<<PD3 );
if (servo3 == i)
PORTD &= ~( 1<<PD5 );
if (servo4 == i)
PORTD &= ~( 1<<PD6 );
if (servo5 == i)
PORTD &= ~( 1<<PD7 );
/* for (w=0; w<= WAIT; w++){
asm volatile ("nop");
} */
}
PORTD &= 0x13; //Alle Servopins LOW
count= 0;
return;
}
Lesezeichen