#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/sleep.h>
//----------------------------------------------------------------------------
typedef unsigned char byte;
//----------------------------------------------------------------------------
volatile int No=0;
volatile unsigned char Flag_motor=0xff;
//----------------------------------------------------------------------------
#define SERVO_0        (1400*8)    
#define    SERVO_1        (1500*8)
#define SERVO_2        (1340*8)  
#define SERVO_3        (1500*8)
#define SERVO_4        (1330*8)
#define SERVO_5        (1500*8)    
#define SERVO_6        (1240*8)
#define SERVO_7        (1500*8)
//----------------------------------------------------------------------------
#define FACTOR_0    1030
#define FACTOR_1    1024
#define FACTOR_2    1100
#define FACTOR_3    1024
#define FACTOR_4    1000
#define FACTOR_5    1024
#define FACTOR_6    930
#define FACTOR_7    1024
//----------------------------------------------------------------------------
#define S0            0x01
#define S1            0x02
#define S2            0x04
#define S3            0x08
#define S4            0x10
#define S5            0x20
#define S6            0x40
#define S7            0x80
//----------------------------------------------------------------------------
volatile int Data[8][5];
#define Pulse(n)    Data[n][0]    
#define Deg(n)        Data[n][1]    
#define Speed(n)    Data[n][2]    
#define Init(n)        Data[n][3]    
#define Factor(n)    Data[n][4]    
//----------------------------------------------------------------------------
#define sbi(port,bit)  port |=  (1<<(bit))
#define cbi(port,bit)  port &= ~(1<<(bit))
#define outw(a, b) (a = (b))
#define outp(a, b) (b = (a))
//----------------------------------------------------------------------------
// Если минимум 1 то около 0.7градусов/сек
// Если максимум 100 то около 70градусов/сек
//----------------------------------------------------------------------------
// ТАЙМЕР 0
//----------------------------------------------------------------------------
SIGNAL(SIG_OVERFLOW0)
{
    outw(TCNT1L, 0);    
    outp(178,TCNT0);        // 256-178=78 -> 78*32usec=2.496msec

    sbi(PORTD, No);

    if( Pulse(No) > (Init(No)+Deg(No)*71+(Speed(No)/2)) )    Pulse(No)-=Speed(No);
    else if( Pulse(No) < (Init(No)+Deg(No)*71-(Speed(No)/2)) )     Pulse(No)+=Speed(No);
    else     Flag_motor&=~(1<<No);
    
    outw(OCR1A, Pulse(No));        
    sbi(TIMSK, OCIE1A);        // set OCIE1 : Timer1 Output Compare Interrupt enable

    No++;
    No&=0x07;

    cbi(TIFR, TOV0);        // clear OCIE1 : Output Compare Flag1 clear
}
//----------------------------------------------------------------------------
// ТАЙМЕР 1
//----------------------------------------------------------------------------
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
    outp(0, PORTD);            //Clear Port D
    cbi(TIMSK, OCIE1A);       // clear OCIE1 : Timer1 Output Compare Interrupt disable
}
//----------------------------------------------------------------------------
// ИНИЦИАЛИЗАЦИЯ ПОРТОВ
//----------------------------------------------------------------------------
void port_init(void)
{
    outp( 0xfe, DDRB );    
    outp( 0xff, DDRC );    
    outp( 0xff, DDRD );    

    outp( 1, PORTB );    
    outp( 0, PORTC );
    outp( 0, PORTD );
}
//----------------------------------------------------------------------------
// ИНИЦИАЛИЗАЦИЯ СЕРВО
//----------------------------------------------------------------------------
void motor_init(void)
{
    outp(4, TCCR0);            // TIMER_0 CK/256 -> 1=32usec

    outp(0, TCCR1A);
    outp(1, TCCR1B);        // CTC1 clear, TIMER_1 CK/1 -> 8=1usec

    outw(OCR1A, 0xffff);    // 0xffff=65535 -> (65535/8)usec=8.192msec
}
//----------------------------------------------------------------------------
// ВКЛЮЧИТЬ СЕРВО
//----------------------------------------------------------------------------
void motor_enable(void)
{
    outp(178,TCNT0);   // 256-178=78 -> 78*32usec=2.496msec
    outw(TCNT1L, 0);

    cbi(TIFR, TOV0);    // clear TOV0 : Timer0 Overflow Flag clear
    sbi(TIMSK, TOIE0);    // set TOIE0 : Timer0 overflow Interrupt enable

    cbi(TIFR, OCF1A);    // clear OCIF1 : Timer1 Output Compare A Mach Flag clear
    sbi(TIMSK, OCIE1A); // set OCIE1A : Timer1 Output Compare A Mach Interrupt Enable

}
//----------------------------------------------------------------------------
// ОТКЛЮЧИТЬ СЕРВО
//----------------------------------------------------------------------------
void motor_disable(void)
{
    outw(TCNT1L, 0);
    outp(0, TCNT0);
    outp( 0, PORTD );
    cbi(TIMSK, OCIE1A); // clear OCIE1A : Timer1 Output Compare Interrupt disable
    cbi(TIMSK, TOIE0);    // clear TOIE0 : Timer0 overflow Interrupt disable
}
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
void motor_zero(void)
{
    int i;

    Init(0)=SERVO_0;
    Init(1)=SERVO_1;
    Init(2)=SERVO_2;
    Init(3)=SERVO_3;
    Init(4)=SERVO_4;
    Init(5)=SERVO_5;
    Init(6)=SERVO_6;
    Init(7)=SERVO_7;

    Factor(0)=FACTOR_0;
    Factor(1)=FACTOR_1;
    Factor(2)=FACTOR_2;
    Factor(3)=FACTOR_3;
    Factor(4)=FACTOR_4;
    Factor(5)=FACTOR_5;
    Factor(6)=FACTOR_6;
    Factor(7)=FACTOR_7;

    for(i=0; i<8; i++)
    {
        Deg(i)=Speed(i)=0;
        Pulse(i)=Init(i);
    }
}
//----------------------------------------------------------------------------
// ИНИЦИАЛИЗАЦИЯ
//----------------------------------------------------------------------------
void system_init(void)
{
    port_init();
    motor_init();
    motor_zero();
}
//----------------------------------------------------------------------------
// ЖДАТЬ ПОКА ПРОИИСХОДИТ ДВИЖЕНИЕ
//----------------------------------------------------------------------------
void wait_until_move(void)
{
    Flag_motor=0xff;
    while(Flag_motor);
}

//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
void absolute_deg(int no, int deg, int speed)
{
    Deg(no)=(int)((long)(deg)*(long)(Factor(no))>>10);
    Speed(no)=(int)((long)(speed)*(long)(Factor(no))>>10);
}
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
void same_absolute_deg(byte flag, byte dir, int deg, int speed)
{
    int i;

    wait_until_move();
    for(i=0; i<8; i++)
    {
        if( flag&(1<<i) )
        {
            if(dir&(1<<i))    absolute_deg(i, deg, speed);
            else            absolute_deg(i, -deg, speed);
        }
    }
}
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
void relative_deg(int no, int deg, int speed)
{
    int temp;

    temp=(int)(((long)(Deg(no))<<10)/(long)(Factor(no)));
    deg+=temp;
    Deg(no)=(int)((long)(deg)*(long)(Factor(no))>>10);
    Speed(no)=(int)((long)(speed)*(long)(Factor(no))>>10);
}
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
void same_relative_deg(byte flag, byte dir, int deg, int speed)
{
    int i;

    wait_until_move();
    for(i=0; i<8; i++)
    {
        if(flag&(1<<i))
        {
            if(dir&(1<<i))    relative_deg(i, deg, speed);
            else            relative_deg(i, -deg, speed);
        }
    }
}

//----------------------------------------------------------------------------
//ГЛАВНАЯ ФУНКЦИЯ
//----------------------------------------------------------------------------
int main(void)
{
    system_init();
    motor_enable();
    sei();                    // Global interrupt enable
    
    while(1)
    {
    absolute_deg(S0, 90, 70);// к примеру..    
    }

}