#include <mega16.h>
#include <delay.h>

#define MaxServo 3      // Число сервомашинок

//============== ПЕРЕМЕННЫЕ ==============
char servo_state;                  // Состояние конечного автомата.
char ServoPortState[MaxServo+1];   // Значение порта которое надо вывести
char ServoNextOCR[MaxServo+1];       // Время вывода значения
char servo_need_update;

 typedef struct
 {
   char Position;
   char Bit;
 }  SArray_def;
 
SArray_def Servo[MaxServo];
SArray_def *Servo_sorted[MaxServo];

//================== ФУНКЦИИ ==============
void Servo_sort(void);
void Servo_upd(void);
void Servo_Init(void);
//Теперь можно писать из прикладной программы любые произвольные значения (26…76 в данном случае) в поле Position, задавая произвольное положение любой из восьми сервомашинок.
//После изменения любого из полей, перед отправкой таблиц в конечный автомат, данные надо упорядочить по длительности. Это делает спец функция.

//Простейший алгоритм сортировки вставкой. Недалеко ушел от пузырька, но на столь малых количествах
// данных являетя наиболее эффективным.
void Servo_sort(void)
{
char i,k;
SArray_def *tmp;
 
// Сортируем массив указателей.
for(i=1;i<MaxServo;i++)
   {
      for(k=i;((k>0)&&(Servo_sorted[k]->Position < Servo_sorted[k-1]->Position));k--)
      {   
         tmp = Servo_sorted[k];                           // Swap [k,k-1]
         Servo_sorted[k]=Servo_sorted[k-1];
         Servo_sorted[k-1]=tmp;
      }
 
   }
}

//Массив отсортирован. Теперь надо его обработать и сформировать таблицы для автомата.
//Обработка заключается в склейке совпадающих полей. Т.е. если у нас есть, например, 5 каналов с длительностью 20,
//то их надо склеить в одно значение таблицы, а биты слепить в единую битмаску.
//А потом загрузить все в массивы конечного автомата.
//Обновление идет в функции Servo_upd

void Servo_upd(void)
{
   char i,j,k;
 
   for(i=0,k=0;i<MaxServo;i++,k++)
  {
   if(Servo_sorted[i]->Position!=Servo_sorted[i+1]->Position)   //Если значения уникальные
   {
      ServoNextOCR[k] = Servo_sorted[i]->Position;            // Записываем их как есть
      ServoPortState[k+1] = Servo_sorted[i]->Bit;            // И битмаску туда же
   }
   else                                               // Но если совпадает со следующим
   {
      ServoNextOCR[k] = Servo_sorted[i]->Position;           // Позицию записываем
      ServoPortState[k+1] = Servo_sorted[i]->Bit;           // Записываем битмаску

    // И в цикле ищем все аналогичные позиции, склеивая их битмаски в одну.

      for(j=1;(Servo_sorted[i]->Position == Servo_sorted[i+j]->Position)&&(i+j<MaxServo);j++)
      {
         ServoPortState[k+1] |= Servo_sorted[i+j]->Bit;
      }
      i+=j-1;                                 // Перед выходом корректируем индекс
   }                                         // На глубину зарывания в повторы
  }   
  ServoNextOCR[k] = 0xFF;                          // В последний элемент вписываем заглушку FF.
}

interrupt [TIM0_COMP] void timer0_comp_isr(void)   //совпадение по прерыванию
{
  if (servo_state)                                // Если не нулевое состояние то
    {
       OCR0 = ServoNextOCR[servo_state];          // В регистр сравнения кладем следующий интервал
       PORTA &= ~ServoPortState[servo_state];     // Сбрасываем биты в порту, в соответствии с маской в массиве масок.
      servo_state++;                          // Увеличиваем состояние автомата
       
     if (OCR0 == 0xFF)                          // Если значение сравнения равно FF значит это заглушка
      {                                      // И мы достигли конца таблицы. И пора обнулить автомат
         servo_state = 0;                       // Выставляем нулевое состояние.
         TCNT0 = 105;                           // Программируем задержку в 20мс (на предделителе 1024)
         TCCR0 = 0x00;                   // Сбрасываем предделитель таймера
         TCCR0 |= 0x05;                         // Устанавливаем предделитель на 1024

      if (servo_need_update)                   // Если поступил приказ обновить таблицы автомата
         {
            Servo_upd();                       // Обновляем таблицы.
            servo_need_update = 0;            // Сбрасываем сигнал обновления.
         }
      } 
   }
     else                                    // Нулевое состояние автомата. Новый цикл
        { 
           OCR0 = ServoNextOCR[servo_state];     // Берем первую выдержку.   
           TCCR0 = 0x00;               // Сбрасываем предделитель
           TCCR0 |= 0x04;                      // Предделитель на 256
           PORTA = 0b00000111;                  // Выставялем все сервоканалы в 1 - начало импульса   4
           servo_state++;                      // Увеличиваем состояние конечного автомата.
        }
}

void Servo_Init(void)
{

Servo_sorted[0] = &Servo[0];
Servo_sorted[1] = &Servo[1];
Servo_sorted[2] = &Servo[2];

Servo[0].Bit = 0b00000001;
Servo[1].Bit = 0b00000010;
Servo[2].Bit = 0b00000100;

Servo[0].Position = 50;
Servo[1].Position = 50;
Servo[2].Position = 50;

}

void main(void)
{
 DDRB &=~(1<<PORTB2);
 PORTB |= (1<<PORTB2);
 PORTA = 0b00000111;
 DDRA = 0b00000111;
 Servo_Init();
 TIMSK |= (1<<OCIE0);   
TCCR0 |= 0x04;    //предделитель 256
#asm("sei")
Servo_sort();
while (1)
      { 

        Servo[0].Position =76;
        Servo_upd();
        servo_need_update = 1;
      }
}