Помогите с сервоприводом.

#include <avr/interrupt.h>
#include <avr/io.h>

volatile char pwm_counter,pwm_r,pwm_g,pwm_b;

/*** прерывание по переполнению Т0 ***/
ISR (TIMER0_OVF_vect)
{
if (pwm_counter++ > 63)
{
PORTB = 0x00;
pwm_counter = 0;
}
if (pwm_counter > pwm_r)
PORTB |= (1 << PB1);
if (pwm_counter > pwm_g)
PORTB |= (1 << PB2);
if (pwm_counter > pwm_b)
PORTB |= (1 << PB3);
}

/*** процедура задержки в микросекундах ***/
void delay_us(unsigned char time_us)
{ register unsigned char i;

for(i = 0; i < time_us; i++) // 4 цикла
{ asm (" PUSH R0 "); // 2 цикла
asm (" POP R0 "); // 2 цикла
// 8 циклов = 1 us для 8MHz
}
}
/*** процедура задержки в миллисекундах ***/
void delay_ms(unsigned int time_ms)
{ register unsigned int i;

for(i = 0; i < time_ms; i++)
{ delay_us(25);
delay_us(25);
delay_us(25);
delay_us(25);

}
}
/*** красный цвет ***/
void red (unsigned int time)
{
for (char a = 0; a<165; a++)
{
pwm_r = 164 - a; //увеличение
pwm_g = 164;
pwm_b = 164;
delay_ms(time);
}
for (char a = 0; a<165; a++)
{
pwm_r = a; //уменьшение
pwm_g = 164;
pwm_b = 164;
delay_ms(time);
}
}



/*** красный цвет 2***/
void red2 (unsigned int time)
{
for (char a = 0; a<165; a++)
{
pwm_g = 164 - a; //увеличение
pwm_r = 164;
pwm_b = 164;
delay_ms(time);
}
for (char a = 0; a<165; a++)
{
pwm_g = a; //уменьшение
pwm_r = 164;
pwm_b = 164;
delay_ms(time);
}
}



int main (void)
{
DDRB = (1 << PB3)|(1 << PB2)|(1 << PB1);
// PB3,2,1 - выходы
TIMSK |= (1 << TOIE0);
// разрешение прерывания
TCCR0 |= (0 << CS02)|(0 << CS01)|(1 << CS00);
// без предделителя
sei();// глобальное разрешение прерываний

while(1)
{
red(5);
red2(5);



}
}