Управление множеством сервомашинок

При построении разных роботов порой приходится использовать несколько сервоприводов. А если это какой-нибудь шестиногий паук, то приводов там этих просто тьма. Как ими управлять? На форуме кое кто даже сокрушался, что ему бы для этих целей плисину применить. Хотя на кой черт там ПЛИСка, когда с рулением даже трех десятков сервоприводов справится самый рядовой микроконтроллер, затребовав под это дело всего один таймер.

Итак, кто не помнит как управляются сервы может прогуляться в старую статью и освежить знания.

Возьмем, для начала, 8 сервомашинок. На каждую серву идет вот такой сигнал:

На каждую серву со своей ноги контроллера должна идти такая вот последовательность. Итого подобие ШИМ’a на 8 каналов. Как сгенерировать эту бодягу? Да проще простого. Принцип тут простой. Импульсы медленные — всего то 50Гц, меняются тоже нечасто — серва штука инерционная, поэтому даже сто раз в секунду ей не подергаешь. Так что времени на обработку у нас вагон и маленькая тележка.

Сами импульсы будут генерироваться одним таймером, в фоновом режиме. Принцип генерации прост: Все импульсы стартуют одновременно, выставляя свои уровни в 1.
Затем в таймер, в регистр сравнения, заносится время длительности первого импульса. По прерыванию сравнения происходит:

  • Сброс бита на порту первого канала
  • Загрузка в регистр сравнения таймера значения длительности второго импульса


И таймер продолжает считать дальше. Загружая по очереди длительности импульсов и отрабатывая все каналы. Когда досчитает до конца, то надо настройкой прескалера замедлить его ход и отщелкать 20мс, а затем повторить цикл.

Таким образом, у нас все восемь каналов формируются в прерывании конечным автоматом, который сам себя загружает, сам сбрасывает биты и так далее. Практически не тратя временные ресурсы контроллера, беря на себя управление только тогда, когда надо сменить уровень на ноге, а это очень малая по времени операция и очень редкое, в масштабах контроллера, событие.

Есть лишь одна деталь — для успешной работы каналы серв должны быть сортированы по возрастанию. Чтобы можно было просто последовательно загружать значение и все.

Конечный автомат, висящий на прерывании сравнения состоит из двух таблиц с данными:

1
2
3
4
#define MaxServo 8		// Число сервомашинок
u08 servo_state=0;		// Состояние конечного автомата. 
u08 ServoPortState[MaxServo+1];	// Значение порта которое надо вывести
u08 ServoNextOCR[MaxServo+1];	// Время вывода значения

ServoPortState — содержит биты которые надо сбросить при каждом временном интервале. То есть если у нас два сервоканала имеют одинаковую длительность импульсов (т.е. сервы выставлены в одно и то же положение), то они будут сброшены одновременно. За один проход.
ServoNextOCR — таблица длительностей сигналов. Тут в порядке возрастания собраны длительности отчетных интервалов.

И код автомата:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
ISR(TIMER0_COMP_vect)				// Прерывание по совпадению
{
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 &= 0b11111000;		// Сбрасываем предделитель таймера
		TCCR0 |= 0x05;			// Устанавливаем предделитель на 1024
 
		if (servo_need_update)		// Если поступил приказ обновить таблицы автомата
			{
			Servo_upd();		// Обновляем таблицы.
			servo_need_update = 0;	// Сбрасываем сигнал обновления.
			}
		}
	}
else						// Нулевое состояние автомата. Новый цикл
	{
	OCR0 = ServoNextOCR[servo_state];		// Берем первую выдержку.
 
	TCCR0 &= 0b11111000;			// Сбрасываем предделитель
	TCCR0 |= 0x04;				// Предделитель на 256
	PORTA = 0xFF;				// Выставялем все сервоканалы в 1 - начало импульса
 
	servo_state++;				// Увеличиваем состояние конечного автомата. 
	}
}

Вот такой вот незамысловатый алгоритм. Это касаемо генерации самих импульсов. Но ведь нам надо еще каким то образом сделать таблицы длительностей и битмасок, которые будут формировать сигнал. Да еще обеспечить человеческий интерфейс взаимодействия с конечным автоматом. Этим и займемся :)

Управление сервомашинками осуществляется через структуру под которую мы даже зарядил свой тип:

1
2
3
4
5
6
7
typedef struct 	{
		u08 Position;
		u08 Bit;
		}  SArray_def;
 
SArray_def Servo[MaxServo];
SArray_def *Servo_sorted[MaxServo];

Servo.bit — содержит бит порта,который нам надо сбросить. Просто единичка в нужном разряде.
Servo.Position — это позиция сервопривода. В моем случае может принимать значения от 26 до 76. Если больше-меньше, то серва уйдет за границы допустимых положений, что черевато. Можно было поиграться с частотой процессора и предделителями, чтобы выбрать диапазон побольше, но мне было лень. В конце концов, для демонстрации возможностей 50 положений вполне хватит. А желающие могут и на 16ти разрядный таймер все перевесить и будет у них точность до долей градуса :)

Так как сервомашинки надо сортировать, то я сделал еще массив указателей Servo_sorted. Чем ворочать в памяти неповоротливые структуры, я лучше перетасую в порядке возрастания поля Position их указатели. Сожрет лишнюю память, но выйграет вы быстродействии.

Впрочем, в данном случае, когда структура имеет всего два восьмибитных поля, экономия выглядит сомнительной, т.к. тот же указатель занимает те же самые два байта. В лучшем случае баш на баш. Но кто сказал, что структура будет неизменной? Может что еще захотим добавить! Поэтому пусть лучше сортируются указатели.

Для начала указатели и структуры инициализируются нужными данными:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
// Присваиваем указателям адреса наших структур. 
Servo_sorted[0] = &Servo[0];
Servo_sorted[1] = &Servo[1];
Servo_sorted[2] = &Servo[2];
Servo_sorted[3] = &Servo[3];
Servo_sorted[4] = &Servo[4];
Servo_sorted[5] = &Servo[5];
Servo_sorted[6] = &Servo[6];
Servo_sorted[7] = &Servo[7];
 
// Заполняем битмаски
Servo[0].Bit = 0b00000001;
Servo[1].Bit = 0b00000010;
Servo[2].Bit = 0b00000100;
Servo[3].Bit = 0b00001000;
Servo[4].Bit = 0b00010000;
Servo[5].Bit = 0b00100000;
Servo[6].Bit = 0b01000000;
Servo[7].Bit = 0b10000000;
 
// Выставляем все положения на нейтраль -- точно посредине диапазона. 
Servo[0].Position = 50;
Servo[1].Position = 50;
Servo[2].Position = 50;
Servo[3].Position = 50;
Servo[4].Position = 50;
Servo[5].Position = 50;
Servo[6].Position = 50;
Servo[7].Position = 50;

Теперь можно писать из прикладной программы любые произвольные значения (26…76 в данном случае) в поле Position, задавая произвольное положение любой из восьми сервомашинок.

После изменения любого из полей, перед отправкой таблиц в конечный автомат, данные надо упорядочить по длительности. Это делает спец функция.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
//Простейший алгоритм сортировки вставкой. Недалеко ушел от пузырька, но на столь малых количествах
// данных являетя наиболее эффективным. 
void Servo_sort(void) 
{
u08 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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
void Servo_upd(void)
{
u08 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.
}

Т.к. менять таблицы автомата до тех пор пока он не отработает до конца черевато глюками и ахтунг работой, то делать обновление можно только только тогда, когда автомат выйдет на нулевое положение. Поэтому я повесил это дело в прерывание. А в прикладной программе оставил только обновление структур и их сортировку. И добавил флаг готовности servo_need_update.

Т.е. если нам надо изменить позицию сервомашинки, то мы пишем следующую комбинацию:

1
2
3
Servo[Б].Position =А
Servo_sort();
servo_need_update = 1;

И при следующем проходе автомата сервомашинка Б выйдет на позицию А. Можно одновременно менять хоть все машинки сразу.

Я сделал небольшую демонстрашку этой программы. Через диспетчер пустил по восьми сервам синусоидальные волны. Если сделать робоглиста подобного тому, что уже описывался в моем блоге, то наверное он поползет по змеиному. Впрочем, я не уверен в этом. Т.к. сколько я не наблюдал змей никак не могу понять физическое обоснование их движению. Она словно по земле течет.

Сама программа, гонящая синус по сервам, сделана на диспетчере и выглядит так:

1
2
3
4
5
//Sinus
u08 SinState=0;			// Состояние генератора
 
// Таблица синуса, приведенная к нашим допустимым значениям
u08 SinTable[] PROGMEM = {76,74,68,60,50,41,33,27,26,29,35,43,53,62,70,75};
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
void Sinus(void)
{
 
SetTimerTask(Sinus,100);			// Перезапуск задачи через 100мс
 
Servo[0].Position = pgm_read_byte(&SinTable[SinState&0x0F]);
Servo[1].Position = pgm_read_byte(&SinTable[(SinState+1)&0x0F]);
Servo[2].Position = pgm_read_byte(&SinTable[(SinState+2)&0x0F]);
Servo[3].Position = pgm_read_byte(&SinTable[(SinState+3)&0x0F]);
Servo[4].Position = pgm_read_byte(&SinTable[(SinState+4)&0x0F]);
Servo[5].Position = pgm_read_byte(&SinTable[(SinState+5)&0x0F]);
Servo[6].Position = pgm_read_byte(&SinTable[(SinState+6)&0x0F]);
Servo[7].Position = pgm_read_byte(&SinTable[(SinState+7)&0x0F]);
 
SinState++;					// Увеличиваем состояние
 
Servo_sort();					// Сортируем таблицы
servo_need_update = 1;				// Запрос на обновление автомата
}

Обратите внимание на реализацию генерации синуса. У меня в таблице синуса (вообще то там косинус, но пофигу) есть только один период. Из 16 значений. А у нас восемь серв на которые должны идти последовательные значения из этого ряда:

  • n
  • n+1
  • n+2
  • n+3
  • n+4
  • n+5
  • n+6
  • n+7

Но т.к. период всего один и вывод надо закольцовывать. Т.е. если n+m вылазит за границу длины нашего масива, то его надо завернуть в начало. Если делать это математическим методом условий, то будет каша с кучей проверок.

Я, как бывалый ассемблерщик, решил все проще, применив трюк с ограничением разрядности битмасками. Т.е. в массиве у нас 16 значений. Поэтому если мы n+m обрежем до 4 бит маской, то оно в принципе не сможет вылезти за границы в 16 (0x0F) элементов и так и будет вращаться по кругу. Даже проверку на if (n==7) n=0; делать не надо. Усе будет автоматически!

Данный метод должен прорулить и в кольцевых буферах, кратных 4, 8, 16, 32, 64, 128, 256 байтам. Просто записываем базу, а потом имеем смещения головы и хвоста, ограниченные в разрядности, чтобы они автоматом не вылезали за пределы кольца.

Конструкция pgm_read_byte — это чтение из флеша. Подробно я ее уже расписывал в статье про работу с памятью

Проблемы
Главная проблема при такой генерации — это джиттер. Т.е. дрожание фронтов. Возникает из-за того, что прерывания в AVR имеют одинаковый приоритет и кто раньше встал того и тапки. А если в системе есть еще один таймер, генерящий свое прерывание? То он будет создавать задержки для конечного автомата, из-за чего поплывут фронты. Что черевато. У меня такое прерывание есть — таймер диспетчера задач с его службой времени. Я решил проблему просто — диспетчер рулит прикладными задачами, поэтому его приоритет может быть и ниже автомата, который обеспечивает хардверную реализацию фичи.

Поэтому я разрешил прерывания в обработчике прервыаний диспетчера. Получились вложенные прерывания. В принципе ничего страшного, главное понимать, чем это грозит. А грозит это зарыванием в прерываниями — это когда прерывание вызывается чаще чем обрабатывается. И более глубоким загрузом стека. Зарывание исключено — Служба таймеров обрабатывается в пару порядков быстрей чем вызывается, а вот от двойной нагрузки на стек никуда не деться. Но это можно просто учесть.

Архив с проектом для WinAVR+GCC

Скомпилировали, зашили, запустили…
Получается такая картина на выходах портов:

Дофига серв у меня, к сожалению, нет. Есть всего одна. Зато есть осциллограф с логическим анализатором. Поэтому я его просто подключил к схеме и снял видяшку:

Данную программную конструкцию можно легко расширить хоть до сотни серв. Главное лишь бы портов хватило. Надо всего лишь изменить разрядность данных отвечающих за битмаски, а в конструкции автомата, в части где идет вывод:

1
	PORTA &= ~ServoPortState[servo_state];

Добавить еще портов и выводить на них соответствующие байты многобайтной битмаски. Можно это сделать маской-сдвигом, но наверное лучше будет через указатель вытаскивать нужные байты.

И при этом у нас остается еще прорва процессорного времени на выполнение других задач!

З.Ы.
Оптимизацией кода практически не занимался, так что там еще многое что следует доточить. Начиная от механизма инверсии битов и заканчивая оптимизацией всех структур.

129 thoughts on “Управление множеством сервомашинок”

      1. Я могу подхватить по HDL’ам. С нуля писать вряд ли соберусь, да и не паял я под них платы никогда. А вот пописал под них будь здоров.
        Ну по Xilinx тоже могу порассказать.

  1. На ПЛИС это выглядело бы примерно так:
    Делитель частоты вырабатывает импульсы 50 Гц, которые обнуляют и запускают счетчик.
    Счетчик сделан так, что переполняется за 2,5 ms. Разрядность берется какая надо, а скорость подбирается.
    К выходу счетчика приделано N элементов сравнения, а их выходы идут к сервам.
    Как только счетчик обнулился — из сравнивателя вылезла единица.
    Как только счетчик перевалил за константу — сравниватель выплюнул ноль.
    Естественно к сравнивателям подключены регистры, хранящие значения ШИМов. А уже загрузка этих регистров может какая угодно. В общем так можно получить очень дофига каналов. Я для одной задачки делал 96 каналов по 4 бита и это влезло в EPM570T100 и еще место осталось… Загрузка там была по SPI вроде.

    1. Ну если надо плучить быстрые и точные шимы то да, без плиски не обойтись.

      Надо бы изучить ее как нибудь. Хотя бы на уровне спаять-запустить. А там уж надо будет разберусь. Шьется она байтбластером каким нибудь?

  2. Еще вариант, применяется в радиоуправлении, управляем 8-ю сервомашинками «по одному проводу» (точнее, по радиоканалу): http://www.rcdesign.ru/articles/radio/tx_intro#eztoc9044_6 («принципы формирования радиосигнала»), декодер — CD4017 или аналог, включенный по такой схеме: http://skyflex.air.ru/images/glider/gl097.gif Принцип работы описан тут: http://skyflex.air.ru/pages/glider/gl049.shtml

    Собственно, это и есть метод «правильного» включения сервомашинок.

      1. ИМХО, в моделизме хватает и старого доброго PPM. С передачей цифры все довольно тоскливо — у разных производителей — разные стандарты, чем они беззастенчиво пользуются и накручивают цену на свою продукцию.

        Потом, встречал мнения, что цифра — это неспортивно :)

        В общем, вот статейка на тему: http://www.rcdesign.ru/articles/radio/ppm_pcm

              1. Нет, есть, конечно, и телеметрия, и полеты по камере — но пока это остается экзотикой да и поднимать на модели мощный передатчик сложно — батареи и так садятся быстро. На rcdesign обсуждается даже несколько «любительских» устройств телеметрии.

                В любом случае, канал управления и канал телеметрии разнесены. Популярный вариант — полеты по камере с подмешиванием телеметрии в видеосигнал (такие «китайские» видеопередатчики на 2,4 ГГц и приемником, продаются на всех радиорынках), простой OSD делается на AtMega или чем-то подобном.

                Это по поводу «авиации». В остальных видах моделизма — полный ахтунг («автомобилисты», например, в радио не разбираются поголовно).

                1. В принципе, оно ожидаемо. Для них главное пилотажная техника, ну может аэродинамика и механика. А электроника это своего рода конструктор из серии лего. А как оно фурычит внутри мало кого волнует.

                  1. Главное, что и восемь каналов PPM без коррекции ошибок всех устраивают. Телеметрия — в принципе, это игрушка — прикольно, но, летать далеко все равно нельзя (автопилот на модели — ненадежная штука). А так — даже радиоуправляемые планеристы по несколько часов летают на довольно примитивной, по нынешним временам, старой аппаратуре Multiplex (одна из первых «программируемых»), и не признают никакой новомодной цифры. А следят за моделью — естественно, в бинокль.

        1. У меня друг один раз пробежал за моделью около трёх километров, после этого сразу был решён вопрос о переходе на цифру. Модель просто ни с чего врубила полный газ и ушла в поле. Только на машине и смогли догнать, сначала бегуна, а затем и модель.

  3. Алгоритм толковый, я делал по такому же принципу ШИМ на ATmega8535 на 24канала аж по 10бит (8rgb светодиодов) отлично справляется с задачей (еще и на прием данных время остается)

    код хороший (у меня покривее будет:), респект!

        1. не «выжимал», прикинул, что для светодиодов будет нормально 150герц (больше боялся, что ресурса на UART не хватит, поэтому и не разгонялся)

          ЗЫ мерцание видно только когда рукой быстро водишь — пальцы двояться :)

  4. Меня твой телепатический приемник просто наповал убивает: начал я разбираться управлением мощной нагрузкой, — на изиелектрониксе в тот же день появляется статья о этом. Только начинаю ковырять полевики — та же история. Так же было и с сервами. Теперь я начал раскидывать мозгой по поводу двухосевой стабилизационной платформы и думать над управлением двумя сервами одним микроконтроллером — ты пишешь эту статью.
    ВНИМАНИЕ! НАС ПРОСЛУШИВАЮТ!!111111

  5. > А частоту какую выжал?

    Делал на atmega168 управление тиристорами (6 штук) для пуска двигателя и одновременный сбор аналоговой информации (токи, напряжения) по 6 каналам. Таймер был настроен на 10000 Гц, так что длина минимального импульса — 100 мкс, а максимальная частота — 5000 Гц.

    Контроллер обрабатывал поток, в котором было описание управляющего сигнала, а в ответ отдавал оцифровку 6 каналов с частотой 10000 Гц (8 бит на канал).

    Вот картинка момента пуска, вверху виден один из управляющих каналов (низкий уровень означает «включен»), синяя синусойда — это одна из фаз, а красная кривая — это ток.

      1. Все софтверно, используется прерывание таймера и прерывания АЦП. Только они друг другу не мешают. Обмен данными по RS232 (далее на FT232RL) сделан без без прерываний.

    1. А нельзя ли получить немного информации по Вашему проекту ? Возникла необходимость в подобном устройстве (управление асинхронным двигателем тиристорами + контроль тока). Не поделитесь ?

  6. блин! Надо всё-таки взяться да довести до нормального вида вытесняющий RTOS, да выложить на форуме

    имхо, пускаем 8 потоков, да с задержечкой, и вообще шикарно)) к выходным постараюсь причесать)

  7. > А если это какой-нибудь шестиногий паук, то приводов там этих просто тьма.

    Вот там http://roboforum.ru/forum88/topic2053.html народ развлекается гексаподами на одном движке. Кинематика там, конечно, головоломная. Фишка еще и в том, что механика — из канцелярских скрепок.

    Вот только эти букашки даже поворачивать не умеют.

  8. Вчитался, вдумался — клёво.

    А то как-то в одном проекте на работе мой предшественник на один канал ШИМа два таймера задействовал — на период, и на длительность импульса %-).
    Правда, камень был 51 (точнее, 8252), а не авр.

  9. Этсамое, вы чего все? Зачем сортировать по возрастанию? Какие два таймера? Какие 8 потоков?

    Все делаеться на одном таймере:

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    
    void timer1_c_irq_handler(void)
    {
        AT91PS_TC TC_pt = AT91C_BASE_TC1;
        unsigned int dummy;
        //* Acknowledge interrupt status
        dummy = TC_pt->TC_SR;
        //* Suppress warning variable "dummy" was set but never used
        dummy = dummy; 
     
        switch (current_srv)
        {
        case 0:                                                    AT91F_PIO_SetOutput( AT91C_BASE_PIOA, SO1    ); AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 1; break; // - установка 1 сервы: выставили сигнал первой сервы, загрузили длительность ее сигнала, увеличили значение обрабатываемой сервы, вышли
        case 1: AT91F_PIO_ClearOutput( AT91C_BASE_PIOA,     SO1 ); AT91F_PIO_SetOutput( AT91C_BASE_PIOA, SO2    ); AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 2; break; // - установка 2 сервы: почистили первую серву, выставили вторую, загрузили длительность ее отработки
        case 2: AT91F_PIO_ClearOutput( AT91C_BASE_PIOA,     SO2 ); AT91F_PIO_SetOutput( AT91C_BASE_PIOA, SO3    ); AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 3; break; // - установка 3 сервы:
        case 3: AT91F_PIO_ClearOutput( AT91C_BASE_PIOA,     SO3 ); AT91F_PIO_SetOutput( AT91C_BASE_PIOA, SO4    ); AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 4; break; // - установка 4 сервы:
        case 4: AT91F_PIO_ClearOutput( AT91C_BASE_PIOA,     SO4 ); AT91F_PIO_SetOutput( AT91C_BASE_PIOA, SO5    ); AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 5; break; // - установка 5 сервы:
        case 5: AT91F_PIO_ClearOutput( AT91C_BASE_PIOA,     SO5 ); AT91F_PIO_SetOutput( AT91C_BASE_PIOA, SO6    ); AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 6; break; // - установка 6 сервы:
        case 6: AT91F_PIO_ClearOutput( AT91C_BASE_PIOA,     SO6 ); AT91F_PIO_SetOutput( AT91C_BASE_PIOA, SO7    ); AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 7; break; // - установка 7 сервы:
        case 7: AT91F_PIO_ClearOutput( AT91C_BASE_PIOA,     SO7 ); AT91F_PIO_SetOutput( AT91C_BASE_PIOA, SO8    ); AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 8; break; // - установка 8 сервы:
        case 8: AT91F_PIO_ClearOutput( AT91C_BASE_PIOA,     SO8 );                                                 AT91C_BASE_TC1->TC_RC = srv_o[current_srv]; current_srv = 0; break; // почистили 8 серву, загрузили паузу, просто ждем, ничего не меняя
        default: current_srv = 0; //Исключительная ситуация, в теории невозможна, но на всякий случай
        }
    }

    И одной функции:

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    
    void  set_servo(char servo, unsigned int tick) //сервы нумеруются с нуля,  0-7 - положения сервомашинок, 8 - пауза между посылками 
    {
      unsigned int summ = 0; 
     
      srv_o[servo] = tick; // выставили интересующее значение
     
      for(char i=0; i < MAX_SERVOS; i++)    //рассчитали время результатирующей задержки
        summ += srv_o[i];
     
      srv_o[MAX_SERVOS] = 31250 - summ; // 31250 - length of full packet (0.02 sec)
      //выставили как 0.02 сек минус накопленное время
    }

    «Глянув раз — применяю и сейчас». Это к тому, что разберитесь, и идея вам понравится.
    Вот линк на реализацию: http://www.youtube.com/watch?v=1dCFXCIuScs
    Там правда Мега64, и немного недоделанный алгоритм, но принцип тот-же.
    Вот исходник от меги: http://narod.ru/disk/20227676000/Servo_controller.rar.html

    1. Ну да, можно и так. Только у тебя все импульсы серв идут друг за дружкой. Но сколько серв ты так сможешь пустить, чтобы вписать их в интервал 10мс? М? А если у тебя не 8, а 30 серв? Не влезут ведь! Будет запаздывание следующего импульса — как следствие оттормаживание сервопривода.

      КРоме того, если у тебя все сервы стоят в одном положении, то у тебя на каждую, несмотря на все, тратится процессорное время. Ведь они обрабатываются индивидуально, каждая в своем проходе. А в моем варианте все совпадающие по положению сервы обрабатываются за один заход. И разогнать я их еще могу ничего для этого не потеряв.

      1. Вот в репе:
        http://easyelectronics.ru/repository.php?act=view&id=21 — функция
        http://easyelectronics.ru/repository.php?act=view&id=20 — обработчик прерывания

        Процессорное время на обработку прерывания таймера нужно совсем чуть-чуть — переключить ножки. Для функции (установка положения сервы) тоже немного.

        По документации на серву пульсации подаются с частотой 50 Гц — это период 20 мс — значит максимум на один таймер 10 сервоприводов (из рассчета по 2 мс — крайнее положение всех 10 серв).

        1. Дело не только в процессорном времени, а еще в частом вызове прерывания. Это ведь дает джиттеры на другие прерывания. Что черевато и чем реже прерывания случаются, тем лучше.

          Ну и всего 10 серв на один таймер это не так прикольно как несколько десятков на один :)

          1. Да, насчет 10 серв верно — если нужно больше, то только твой алгоритм

            Кстати, количество прерываний, если у всех серв разные значения, будет одинаковым для обоих алгоритмов.
            Выходит эти алгоритмы для разных задач — большое количество серв необходимо, к примеру, для роботов, там движения серв могут быть практически дискретными(поднять/опустить ногу), и достаточно редкими — раз в секунду, поэтому пересортировки не столь затруднительны

            А 8~10 серв(в максимуме) требуются для авиамоделей/etc, там на сервы нагрузка большая, и тратить время на пересортировки не желательно. Собсно поэтому я и пользуюсь им.

            1. Простите, что вмешиваюсь в давно завершенный разговор.

              Относительно недавно заинтересовался радиоэлектроникой и возникла необходимость управлять несколькими сервами одновременно. Начинал свои поиски как раз с этой статьи.

              В моем случае заведомо известно, что это будет априори не предсказуемый сигнал и делать сортировку перед каждым рассчетом для сервы — расточительство.

              Но к сути. Вариант решения — похож на метод описанный выше, но не совсем:

              У сервы полезный сигнал длиться ~2 ms, затем промежуток в 20ms — ~2ms = ~18 ms.

              Практически (ассемблерной вставкой) удалось добиться ровно 6 тактов (могу ошибиться в точной цифре) на цикл «расчета» длины сигнала для одной сервы, уложившись примерно в шаг 0.7 градуса (для 180), без «дрожания» из-за нестабильно кол-ва тактов в цикле.

              Метод вызывается прерыванием по-таймеру. Далее поочередно для каждой сервы в цикле считается время подачи сигнала.

              Таким образом можно без погрешностей из-за «дрожания» сигнала управлять примерно 8 сервами.

              Для наглядности, генерация сигнала для 3х серв:
              (1)-| 2 ms |————————
              (2)————| 1 ms |—————
              (3)———————| 1.5ms |-

              Если интересно, то могу попробовать восстановить код =)

                1. Нет. Если на каждую серву давать максимальную ширину, продолжая считать цикл и после снятия сигнала.

                  Но есть другой неприятный эффект: при 8 сервах для обработки логики остается 1-2 ms, поскольку остальное время отсчитывается сигнал серв. Для сложной или критичной ко времени программы может быть неприемлимо. Тогда либо снижать частоту сигнала, например, до 25Гц (на практике разницы не заметил), либо количество серв.

                  Однако, подводные камни может подкинуть компилятор, разворачивая циклы или условия в запутанные дебри с неподдающимся контролю количеством тактов на итерацию.

                  Кроме того некоторые команды сравнения могут занимать плавающее кол-во тактов (BREQ, например, 1 или 2 такта). А поскольку команды атомарны, то и прерывания могут съезжать. На практике это дает «дергающийся» график.

  10. Доброго времени суток!
    Хочу использовать этот алгоритм для управления группой серво.
    Пока собрал в один файл функции
    http://easyelectronics.ru/repository.php?act=view&id=22
    Но мне не нужен RTOS .
    Что нужно еще сделать чтобы пользоваться этим кодом ?
    Servo[Б].Position =А
    Servo_sort();
    servo_need_update = 1;
    Заранее спасибо!

        1. Да в серво инит идет запуск таймера уже. Надо только прерывания разрешить. Ну и еще не стоит зря в бесконечном цикле дергать сортировку. Много времени ест, да и таймер будет зря перегружаться. Только если изменилось чо.

              1. К сожалению из проэкта я выбрал нужное — то — листинг — что я дал ссылку. И при инициализации МК просто пишу запуск сервоинит.
                Также присутствует asm(«sei»);
                Но на выходах постоянно 1

                    1. В коде серво всего в 2х местах упоминается сам порт.
                      JTAG я еще сразу отключил. Это отладочная плата — у меня эммитерные повторители стоят на PORTC — только была программа шаговика.

                    2. Разве что вот так ругается
                      ompiling C: servo.c
                      avr-gcc -c -mmcu=atmega32 -I. -gdwarf-2 -DF_CPU=12000000UL -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enums -Wall -Wstrict-prototypes -Wa,-adhlns=./servo.lst -std=gnu99 -MMD -MP -MF .dep/servo.o.d servo.c -o servo.o
                      servo.c:31: warning: return type defaults to ‘int’
                      servo.c:31: warning: function declaration isn’t a prototype
                      servo.c: In function ‘ISR':
                      servo.c:31: warning: type of ‘__vector_10′ defaults to ‘int’
                      servo.c:48: warning: implicit declaration of function ‘Servo_upd’
                      servo.c: At top level:
                      servo.c:110: warning: conflicting types for ‘Servo_upd’
                      servo.c:48: warning: previous implicit declaration of ‘Servo_upd’ was here
                      servo.c: In function ‘ISR':
                      servo.c:63: warning: control reaches end of non-void function

                    3. Позырь под отладчиком что там творится в периферии и все сразу станет ясно. Для пущей наглядности выруби оптимизацию.

                    4. А в отладчике что происходит? Таймер тикает? Прерывания срабатывают?

                    5. Ну я в чистом ВинАВР гцц.
                      Хотя пожалуй надо пересесть в отладчик.
                      Отключение оптимизации не помогло.
                      Спасибо за внимание. Попробую пересесть в отладчик и там поглядеть.

                    6. Я вообще без отладчика-симулятора и строчки не пишу. Какой смысл сидеть тупить в код если можно тут же эмульнуть и позырить что да как.

                      А если еще и толком не знаешь что делать, то иначе и никак. Так что аврстудия наше все :)

                    7. Значит собрал я всё в кучу. В отладчике.
                      Сервоинит делает. Заводит таймер.
                      Наступает на строку разрешения прерываний,
                      переходит к бесконечному циклу — и прыгает к началу функции main. Потом снова покругу.
                      Если закомментировать разрешение прерываний, то щелкает до переполнения таймера и ничего не делает.

                    8. Сразу прыгает? А флаги прерываний при этом какие нибудь возникают? МОжет ты заинициализировал уарт, оставил ему прерыванияе по опустошению буфера, или еще какое, а самого обработчика нету?

                      Перейди в дизассемблер и позырь к какому вектору прыгает программа. Поймешь что вызывает прерывание которое все ломает.

                    9. Никаких прерываний лишних ничего. Всё убрано.
                      Однако!
                      Когда запускаю на автошаги, по С коду — то твориться таже дребедень. Таймер доходит до 0х05 и на 0х06 снова начинает функцию main .
                      А вот в дизассемблере это выглядит иначе —
                      считает до переполнения и потом долго шариться по 4м строкам
                      +00000054: 921D ST X+,R1 Store indirect and postincrement
                      +00000055: 3AA0 CPI R26,0xA0 Compare with immediate
                      +00000056: 07B1 CPC R27,R17 Compare with carry
                      +00000057: F7E1 BRNE PC-0x03 Branch if not equal
                      И снова функция main.

                    10. у тебя явно где то прерывание разрешено неиспользованное в коде. Ну не может быть такого просто так.

                      Скинь архивом проект для студии на dihalt@dihalt.ru

  11. Доброго времени суток!
    Сделал всё как хотел, принимает с компа посылки, и выставляет значения серв.
    Обнаружил закономерность — при работе с большИми значениями коэф. заполнения частота ШИМ снижается пропорционально. Я так понимаю это из-за того что таймер всегда отсчитывает «еще чуть чуть» перед новым периодом. А если требуется числами от 0-255 управлять заполнением также от почти 0 до почти 1. ?

        1. Таймер может по разному щелкать. Там куча режимов. Причем в своем алгоритме я не парился на заполнение и выравнивание. Т.к. уменя движуха там 0.8 до 2.2 мс и по сравнению с 20мс паузы это никак не роляет. Ну будет частота чуток плавать — не беда.

          У тебя же заполнение от 1 до 255 и либо у тебя импульсы сильно плавают, либо таймер работает не в том режиме. Либо выдержка паузы сделана неверно (частота должна быть около 80-50гц). Т.е. твоя дискретность от 1 до 255 должна давать длину импульса от 0.8 до 2.3 не больше не меньше. Это либо точной подгонкой частоты проца, либо выбором 16ти разрядного таймера. Либо радикально переписывать выдержку, вводя несколько таймерных отсчетов. Иначе никак.

  12. Я в свое время делал сервоконтроллер на 8 машинок. Счас откопал его и попробовал передалать алгоритм: думал, раз по прерыванию таймера, то будет круче.
    Написал все то же самое на АСМе — машинки немного дрожат. + задолбался с изменением положений машинки (у меня их 127).
    Переделал все назад на «шарманку», но украл идею ДИ с битовыми масками — все машинки стоят мертво. Вообще никакого дрожания, абсолютная тишина и покой.

    Хотя, с другой стороны, если контроллер занимается не только машинками, то шарманку не поставить — времени свободного не будет. Но у меня ничего кроме машинок он не контроллит:)

  13. DI HALT
    Как преобразовать на мк одну внешнюю частоту в другую? Допустим 1МГц в 80Гц. Таймер отсчитывает 12500 — на выходе сигнал высокого уровня, далее отсчитывается вторые 12500 импульсов, на выходе должен быть низкий уровень и т.д. — получаем частоту 80. Как это реализовать?
    1. Какое максимальное число для счетчика в мк 1200?
    2. Как можно периодически обнулять счетчик по достижении нужного числа (в данном случае 12500)?

  14. У меня вопрос.Вообщем нужно управлять 150 диодами с помощью микроконтроллера ATMEGA16L 50-pin,какая микросхема при подключении к выходному порту микрокнотроллера сможет преобразовывать двоичный код с него в сигналы на своих ножках?
    Заранее спасибо!

  15. Как вариант. Если жалко ресурсы мк, а сервами управлять надо:

    1. Сколько серв нужно? Выделяем порт под сервы. Максимум будет 8(10*) серв.
    2. Какая точность нужна? Для меги 8 используем либо таймер 1, либо таймер 2.

    Настраиваем нужный таймер на ‘Fast PWM’ и частоту 400Гц как можно точнее(каждые 2.5мс)*
    Включаем прерывания таймера ‘Output Compare match’ и ‘Overflow’
    Создаем массив SERVO[], где будем хранить значения OCR для каждой сервы.

    Теперь нужно будет циклически в прерывании таймера по переполнению ‘Overflow’ :
    >обновлять OCR для следующей сервы
    >устанавливать высокий уровень на текущей ножке, если текущий SERVO[i] больше минимально допустимого значения (иначе можем поломать серву)

    А в прерывании таймера ‘Output Compare match':
    >устанавливать низкий уровень на текущей ножке

    ////вот как это выглядит для таймера 2////
    //порт сервоконтроллера
    #define SERVOPORT PORTD
    //маска сервоконтроллера
    #define SERVOMASK 0xF0

    ISR( TIMER2_OVF_vect )
    {
    TCNT2 = TCNT2_VAL;//400Гц
    servo_i = servo_next;
    servo_next++; if( servo_next>7 ){ servo_next=0; }
    OCR2 = servo[ servo_next ];//обновляем OCR для следующей сервы
    if( servo[servo_i] < TCNT2_VAL ){ return; }
    sets( SERVOPORT, (1<<servo_i)&SERVOMASK );//устанавливаем высокий уровень на текущем пине сервоконтроллера
    }

    ISR( TIMER2_COMP_vect )
    {
    clrs( SERVOPORT, (1<<servo_i)&SERVOMASK );//устанавливаем низкий уровень
    }

    —————
    *для увеличения количества серв можно соответственно повысить частоту, при этом мы можем потерять в углах поворота серв
    *одного порта уже будет недостаточно, тогда применим неинвертирующий дешифратор 4-в-16
    *естественно, дешифратор можно использовать для управления 8 сервами, используя только 4 пина МК

  16. Доброго времени суток!
    Это снова я. =)
    С кодом разобрался, относительно, всё сделал как хотелось.
    Но настало время расти теперь надо 16серво.
    Я полагаю нельзя просто увеличить длину битовых масок до 16 разрядов, или в фильтрафии 8 на 16 заменить..
    Всеравно порт 16битный выходит.
    Вобщем есть АТмега8, с ногами для серво по 3м портам. Как можно сделать на них вывод серво?

    1. Да никаких проблем. Основной массив с битмасками делаешь на 16 разрядов. Разница только будет у тебя в выводе в порты. Т.е. там где ты битмаски превращаешь в уровни порта.

      Т.е. в строке где у меня

      1
      
      PORTA &= ~ServoPortState[servo_state];	// Сбрасываем биты в порту, в соответствии с маской в массиве масок.

      У тебя будет что то вида:

      1
      2
      
      PORTA & = ~( HIGH(ServoPortState[servo_state]) );
      PORTB & = ~( LOW(ServoPortState[servo_state]) );

      Разумеется данные в ServoPortState будут 16ти разрядными.

      1. Вроде понял, как с 16разрядными регистрами.
        Спасибо. Только осталось на СИ сделать 2 виртуальных порта.
        А то раскидано по 3м.
        Можно сделать ассемблерной вставкой? Или ты не делал случаем адаптацию кода под СИ ?

        1. А зачем заморачиваться? СДелай просто битмаску подлинней (по числу портов, пусть будет не на 16, а на больше разрядов), да только на нужные биты (остальные занули). Правда немного лишку памяти займет, но зато работать будет быстро, т.к. сортируются то все равно указатели.

          1. Я чета подзапутался.
            Вот так укомплектована плата, вернее порты с серво 1-16
            DP5 PD6 PD7 PB0 PB1 PB2 PB3 PB4 PB5 PC0 PC1 PC2 PC3 PD2 PD3 PD4
            Просто так было удобно при разводке, и очень не хочется делать нумерацию серво на колодке не по порядку.
            С двумя портами понятно — а как вывод делать в 3?
            Или как лучше?
            Помоги пожалуйста.

  17. А я попрежнему пилю эту прогу, чем больше пилю тем больше тупею.
    Чтобы на ATmega8 работало примерно также — нам нужен таймер Т0 8 битный?
    Как он в прерывании будет называться?

  18. Как по мне, очередь удобнее реализовать на (одно)связном списке (да, на таком объеме расход памяти немного больше). Но и удобства очевидны — сортировка не требуется (однократный просмотр списка и вставка элемента), проще манипулировать данными, расширять систему проще.

      1. Хм. За счет чего должно упасть быстродействие? Выбор элеменета массива не быстрее выбора элемента структуры по указателю, а вот сортировка (на достаточно большом списке каналов) медленнее будет.

        1. Из списка чтобы взять надо пробежаться по всему списку до нужного элемента. Опять же для изменнения надо будет найти нужный элемент в списке и изменить.

          А сортировка делается всего один раз, при изменении. Так что ее быстродействие не слишком критично.

          1. Зачем каждый раз пробегать по списку?
            typedef struct _PWM_PORT
            {
            u08 channel;
            u08 pwm;
            struct _PWM_PORT *next;
            } PWM_PORT;

            PWM_PORT red = {RED_LED, 15, &green};
            PWM_PORT green = {GREEN_LED, 31, &blue};
            PWM_PORT blue = {BLUE_LED, 63, NULL};

            а в прерывании таймера просто перемещаем указатель на каждой итерации (список отсортирован по возрастанию):
            pPWM = pPWM->next;

            Вместо сортировки — просто линейный просмотр списка и как встречаем элемент с бОльшим значением .pwm, просто меняем два указателя.

            1. Т.е. у тебя на канал по структуре. Хорошо. А как насчет склейки повторяющихся значений, чтобы не было джиттера при переключении одинаковых временных интервалов. У меня вся фишка то как раз в том, чтобы заиметь итоговую таблицу, где сброс битов идет синхронно.

              1. Конечно. Городить связный список без структуры??! =0
                Склейка тоже реализована. Просто занулением поля .pwm и корректировкой .channel. За счет нуля этот канал будет в начале списка и не потеряется. А маска прописывается в соответствующий канал.

  19. Добрый день.
    Я пытаюсь сотворить автомат управления освещением, 17 ламп накаливания (17 каналов) через симисторы и 400 светодиодов (10 каналов по 40 штук) через полевики. Плавное включение, различные комбинации включения и т.п. Симисторы управляются фазо-импульсным методом, светодиоды — ШИМ (на основе этой статьи). Все это хозяйство на Atmega32 (16 МГц). Помогите оценить хватит ли производительности процессора или не пытаться запихнуть все в один кристалл и делать на двух?

  20. А вот у меня наоборот — задача уменьшить кол-во серв до трех.
    Висят они у меня на PORTD, и на этом же порту висит пара кнопок. Так вот, чтобы состояние пинов у этих кнопок не изменялись автоматом, генерящим ШИМ, можно ли сделать так:


    //InitPort
    PORTD = 0b00001100;
    DDRD = 0b10110000;
    ...
    if (servo_state){
    PORTD &= ~((ServoPortState[servo_state]) & ((1<<7)|(1<<5)|(1<<4)));
    ...
    }else{
    PORTD |= (1<<7)|(1<<5)|(1<<4);
    }

    Укажите, пожалуйста,если здесь косяк?
    А еще я никак никак не догоню, как рассчитывается величина управляющего импульса в зависимости от конкретного значения Servo[].Position = 26..76 ?
    И еще вопрос, задача Servo_Init() вызывается один раз? Почему сервы, когда с ними не производится никаких действий, хоть и стоят в среднем положении, но дрожат?

  21. Глянул в Proteus длины генерируемых импульсов — они же выходят вроде как за рамки стандарта управления серво: промежутки между импульсами по 20мс, как и полагается, а длины самих импульсов варьируются от 9 до 68 миллисекунд!

    Если закомментить вызов функции Sinus()

    Почему так? как же именно рассчитывается длина импульса в зависимости от задаваемого значения Position, может дело в предделителе таймера 256, на 1024, когда отсчитывается пауза в 20 мсек все ведь нормально.

  22. картинки к вопросу:
    http://s1.hostingkartinok.com/uploads/images/2012/02/0ae71273379174ca65d1846d3b859c68.jpg

    Если закомментить вызов функции Sinus():
    http://s1.hostingkartinok.com/uploads/images/2012/02/0688d641102c53204f926370babbbad1.jpg

    P.S.: как вставлять картинки прямо в комментарий здесь, какой тэг использовать?


  23. //Clock Config
    #define F_CPU 8000000L проц работает на частоте 8МГц
    //Init Timers
    TIMSK |= 1<<OCIE0; // Включены прерывания по сравнению таймера
    TCCR0 |= 0x04; //Делитель выбран 256

    Это значит, что за секунду счетчик считает 8 000 000/256 = 31250 раз, если мы ему скажем остановиться, досчитав до, например, 50 (OCR0=50), то он потратит времени 50/31250 = 0,0016 секунд. Это как раз длина импульса примерно среднего положения сервы (~1500 uS).
    Если моя серва управляется в интервале, например, 1050..1950 uS, то границы для OCR0=34..60, приблизительно.

    Я верно рассуждаю?

      1. Читал… но эти сервы одни самых крутых: Hi-tec HS-7950TH.
        Грешу на питание: сервы при наивысшей нагрузке (35 кг/см) могут потреблять 4.5А при 7.4В, я их так не гружу, но питание даю 6В с L7806 (его макс 1-1.5А), за 10-15 сек спокойной работы L7806 нагревается так, что рука не терпит, и вроде Uвых падает, пока не дашь остыть. Все питается с комп БП.

        Но вроде как мне раньше не под RTOS удавалось их держать спокойными.
        Быть может что-то с зарыванием в прерывания, описанными в этой статье? Proteus тоже показывает на вирт. осциллографе мелкие колебания длин волн.

        1. Запросто. Большой обработчик прерываний таймера RTOS может дрыгать фронты. Т.к. при совпадении обработчиков будет лажа. Решается разрешением прерываний в обработчике таймера RTOS.

  24. Речь идет об этом?


    void SetTask(TPTR TS)
    {
    if (STATUS_REG & (1<<Interrupt_Flag))
    {
    //Disable_Interrupt;
    Enable_Interrupt;
    nointerrupted = 1;
    }
    ....
    }
    void SetTimerTask(TPTR TS, u16 NewTime)
    {
    if (STATUS_REG & (1<<Interrupt_Flag))
    {
    //Disable_Interrupt;
    Enable_Interrupt;
    nointerrupted = 1;
    }
    }
    inline void TaskManager(void)
    {
    //Disable_Interrupt
    Enable_Interrupt
    ....
    }

      1. Перечитав еще раз статейку: http://easyelectronics.ru/avr-uchebnyj-kurs-operacionnaya-sistema-tajmernaya-sluzhba.html, обратил внимание на то, что если увеличить частоту процессора, то вырастет отношение холостых тактов таймера к времени выполнения процедуры таймерной очереди. Если увеличить частоту кварца с 8Мгц до 16Мгц (ATmega16A-PU) — это может исключить или значительно уменьшить дрожание фронтов?

        К сожалению, я пока не шарю в Асме, а детального описания этой RTOS в СИшной интерпретации не нашел. Даже стал подумать об установке scmRTOS, из-за более детальной документации по ней.

        В поиске входа в прерывание диспетчера таймерной службы я окончательно запутался… :(

        1. Исключить нет. Уменьшить — да.

          Вход в прерывание:

          //RTOS Interrupt
          ISR(RTOS_ISR)
          {
          sei();
          TimerService();
          }

          Собстна там уже прерывания разрешены.

          Если хочешь 100% реалтайма и полного отсутствия дрожания, то тут придется отказываться вообще от какой-либо ОС и делать все на конечных автоматах, уделяя максимум внимания выделению приоритетов прерываний.

          А то и переходить на STM32, где есть возможность делать более приоритетные прерывания.

          1. А если в добавок к разрешению прерываний в диспетчере ещё и запретить прерывания в обработчике прерывания сервы. Что бы прерывание диспетчера не помешало вовремя закончить импульс?

  25. Попробовал заюзать код на 16-битном T1… Вот ведь какая штука! Если использовать прескалер счётчика со значением «256», то квант времени в лучшем случае составит 16МГц/256=0,016мс. Располагая для руления сервой интервалом 0,8-2,3мс=1,5мс, получаем примерно 94 (1,5/0,016) шагов «от и до». Причем неважно, таймер 8 бит или 16. Маловато будет! Если бы можно было работать без прескалера, было бы уже вполне приличные 24000 возможных значений. Но! Проблема этого алгоритма в том, что обработчику прерывания нужно времени значительно больше, чем один такт на переключение. А это вполне возможно, когда разница значений положения для двух любых серв равна единице или близка к ней(новое и старое значения OCR будут различаться на эту самую единицу)… Пока что я подозреваю, что «ахтунг-работа» этого кода при значениях прескалера меньше 256 связана именно с этим, не успевает обработчик закруглиться до прихода нового прерывания. И пока рано откладывать на полку идею об использовании ПЛИС для руления сервами!

      1. Хм. А потянет разгон с кварцем больше 16 МГц?.. Правда, у меня есть USBAsp с контроллером AtMega8L (по даташиту предел частоты 8МГц) но работающим с кварцем на 12 МГц. Но даже это несущественно. Мне бы хотя-б получить квант управления 0,0016мс. Это даст около 1000 позций. Вопрос — как? Частоту оверклокингом до 160МГц не поднять :-p, прескалер уменьшить — обработчик прерываний не уложится в слишком короткий интервал между двух прерываний. (кстати сколько тактов обработчик прерывания расходует на простую загрузку очередного значения в ORC, когда не вызывается»srv_upd»?) Подозреваю, что больше 64-х, особенно учитывая, что грузит 16-битные значения, значит прескалер меньше 256 отпадает… Впрочем это пока гипотезы. Но весьма вероятные, т.к. глюки появляются именно когда значения позиций в массиве отличаются на несколько единиц, то есть таймер отрабатывает между ними 1-2 отсчёта и с низким прескалером новое прерывание генерируется до завершения предыдущего.

          1. «…А желающие могут и на 16ти разрядный таймер все перевесить и будет у них точность до долей градуса…» ;-)
            Хватило бы конечно и 500 позиций. Но когда меньше двухсот это слишком мало. Грубое позиционирование плюс погрешности самой сервы. А переделывать потом всю конструкцию, если окажется что такая точность не прокатит, не айс. Поэтому-то и хочется запас по точности.

          2. После ряда экспериментов остановился на прескалере равном восьми. При этом минимальный шаг позиций при котором наблюдается безглючный PWM-выход, равен 13. На частоте внутреннего генератора 8МГц удобно — миллисекунды умножаем на 100 получаем величину позиции. Получаем цепочку позиций «793,806,819,832, … 2288,2301″ — 116 шагов. На 16МГц соотвесттвенно будет 232. Если подстраиваться под конкретную серву, думаю можно выжать под 300 позиций.
            А если переписать критичную часть кода обработчика прерывания на асме, может будет и ещё лучше — судя по листингу gcc в нем чуть не треть кода занимают инструкции push/pop.
            В остальном — все ОК! Серва не дергается, не дрожит, все позиции стабильно отрабатывает.

  26. Добрый день. Поправте меня. где накосячил. Портировал(попытался) на CVAVR для одной сервы.
    interrupt [TIM2_COMP] void timer2_comp_isr(void)
    {
    // Place your code here
    // PORTB.4=~PORTB.4;
    Interrupt_OFF
    switch(servo_state)
    {
    case(0):
    OCR2 = ServoOCR; // В регистр сравнения кладем интервал
    TCCR2 &= 0b11111000; // Сбрасываем предделитель
    TCCR2|=(1<<CS21)|(1<<CS22); // Предделитель на 256
    TCNT2 = 0;
    PORTB.4 = 1;
    ++servo_state;
    break;
    case(1):
    OCR2 = 156;
    // Программируем задержку в 20мс (на предделителе 1024)
    TCCR2 &= 0b11111000; // Сбрасываем предделитель таймера
    TCCR2|=(1<<CS20)|(1<<CS21)|(1<<CS22); // Устанавливаем предделитель на 1024
    TCNT2 = 0;
    PORTB.4 = 0;
    servo_state=0;
    break;
    }
    Interrupt_OK
    }
    Проблема в том, что не обрабатывается один из блоков настройки таймера(предделитель ), то один то другой в зависимости от перестановки строк выпадает( закоментировал- результата ноль проверял ), но OCR2 воспринимает и там. и там. в чем может быть проблема.

  27. У меня на разобранном принтере на подаче каретки стоит постоянник.
    Можно ли им дочтаточно точно управлять ШИМом БЕЗ обратной связи хотя бы на расстоянии 30 см это орьборотов 60 наверное?
    точность нужна хотябы 0,2мм на это расстояние
    Хочу его использовать при создании 3D принтера, не хочется менять на шаговик ,тк у него шестерня без ограничителя, боюсь ремень будет слетать

Добавить комментарий