Код: Выделить всё
#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;
}
}
