Управление несколькими сервоприводами с высокой точностью на МК ATmega16

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

В моем случае предполагалось использовать четыре привода. Для этого проекта я изготовил небольшую плату:

image

image

Сама плата простая – микроконтроллер Atmega16A-AU, стабилизатор для контроллера, два стабилизатора на приводы и PLS-разъем для подключения приводов. В моем случае на приводы нагрузки почти не было (сервы двигали бумажными деталями) поэтому для приводов хватило одного линейного стабилизатора на небольшом радиаторе.

По управлению приводами я нашел две наиболее интересные статьи у alex-exe и di halt'а. Но в обоих методах есть определенные недостатки. В первом случае: необходимо слишком часто вызывать прерывание, что труднопредсказуемо замедляет выполнение основного цикла и не позволяет получить высокую точность позиционирования. При его реализации привод достаточно сильно дергается, точность установки – около 1 градуса. Собственно, вот видео:



Метод DI HALT’а предполагет формирование по возрастанию, то есть необходимо дополнительно обсчитывать сигналы.

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

Принцип управления сервопривода прост: необходимо с периодом 15-20мс отправлять импульсы с длительностью 0,8-2,3мс. Длительность импульса определяет положение сервопривода. Предлагаемый метод предполагает формирование импульсов на ножках МК одного за другим, как показано на рисунке.

image

Для реализации такого метода достаточно всего одного таймера. Я настроил таймер на увеличение каждую микросекунду. Весь период сигнала – 18000мкс, время на формирование одного импульса – 4500мкс. Прерывание по таймеру при управлении четырьмя сервами вызывается восемь раз. Отсюда вытекает ограничение метода: не получится на один таймер повесить больше 8-ми приводов (за 20мс можно сформировать максимум 8 импульсов длительностью 2,3мс).

Переменные angle1-angle4 это углы в нормированных единицах, меняющиеся в основном цикле программы. Для изменения угла от 0 до 180 градусов они должны меняться от 800 до 2200. Все управление происходит в обработчике прерываний. При первом срабатывании таймера в единицу выставляется нога ответственная за первый привод. Тут же в регистр OCR1A записывается значение угла для этого привода. При следующем срабатывании нога устанавливается в 0 и в регистр OCR1A записывается количество тактов, которое осталось до конца периода управления одним приводом (у меня он выставлен 4500). Затем все то же самое проделывается с ногой, ответственной за второй привод. Таким образом, каждые 4,5мс на одной ноге за другой появляется импульс необходимой длительности, причем с точность до 1мкс! То есть точность установки угла будет меньше 10-ти минут и ограничена характеристиками самого привода. Итак, сам код:

ISR(TIMER1_COMPA_vect)     
	{
	if (takt == 0)
		{
		PORTC |= 0b00000001;
		OCR1A = angle1;
		}
	if (takt == 1)
		{
		PORTC = PORTC & 0b11111110;
		OCR1A = cycle - angle1;
		}
	if (takt == 2)
		{
		PORTC |= 0b00000010;
		OCR1A = angle2;
		}
	if (takt == 3)
		{
		PORTC = PORTC & 0b11111101;
		OCR1A = cycle - angle2;
		}
	if (takt == 4)
		{
		PORTC |= 0b00000100;
		OCR1A = angle3;
		}
	if (takt == 5)
		{
		PORTC = PORTC & 0b11111011;
		OCR1A = cycle - angle3;
		}
	if (takt == 6)
		{
		PORTC |= 0b00001000;
		OCR1A = angle4;
		}
	if (takt == 7)
		{
		PORTC = PORTC & 0b11110111;
		OCR1A = cycle - angle4;
		}
	takt = takt + 1;
	if (takt == 9) takt = 0;
	}


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

Вот видео работы четырех приводов:



Обратите внимание на привод в левом нижнем углу – движения практически не заметно! Этот привод управляет стрелой крана. Добавлю еще про сами приводы. Мне в руки попало четыре разных привода – два аналоговых и два цифровых. Примечательно, что оба аналоговых привода могли работать нормально только пару минут, а потом начинать произвольно дергаться, зажиматься в крайних положениях и т.д., хотя оба привода рабочие – проверял сервотестером. Цифровые приводы выполняли все четко и стабильно по несколько часов подряд (а может и суток). Нареканий от заказчика я не слышал. Для стрелы крана я использовал привод Hi-Tech, который изменял свою позицию с минимальным шагом.

И напоследок – фото макета. Видео, к сожалению, выложить не могу.

image

UPD:
Основная статья теперь хранится здесь.

Similar posts

Ads
AdBlock has stolen the banner, but banners are not teeth — they will be back

More

Comments 34

    0
    >>Видео, к сожалению, выложить не могу.
    а схему?
      0
      Схему в данном проекте я не рисовал, но могу показать плату
      image
        0
        Первая мысль: «где на этой схеме стоит кран?» =) Чем-то неуловимо похоже на макет.
      +1
      Это случайно не для России в миниатюре?
        0
        Нет. Хотя про сам макет могу сказать мало. Моя только электроника. Точно знаю что демонстрирует демонтаж устаревшей электростанции. Типа сначала построили и пустили в эксплуатацию новую, а старую начинают разрушать только после этого.
          +1
          Петард надо побольше! Петард!
        +3
        Макет шикарен!
          +1
          Интересно зачем для атмеги такой суровый предохранитель?
            +1
            Это у него него только вид суровый, а номинал 1А. Через него ведь еще и ток сервоприводов течет!
            +2
            кучу if (takt == ..) можно заменить на один switch, читается лучше
              +1
              Да, действительно. Это неплохая мысль. Я просто скорее электронщик, чем программист.
                +1
                НА куче if получается несколько компактней. По крайней мере на AVR. Впрочем, тут смотреть надо. Может компилятор догадается развернуть кейс через индексный переход. Не всегда, но у него бывают такие озарения. Особенно у IAR.

                  +2
                  Если хочется поменьше условных переходов, то можно еще как-нибудь вот так можно написать.

                  unsigned int servo = takt / 2; // or takt >> 1
                  
                  unsigned char angle = angles[servo];
                  unsigned char mask = 1 << servo;
                  
                  if (takt & 1) {
                          PORTC &= ~mask;
                          OCR1A  = cycle - angle;
                  } else {
                          PORTC |= mask;
                          OCR1A  = angle;
                  }
                  
                  takt = (takt + 1) & 7;
                  
                    0
                    если все выходы висят на одном порту — это, пожалуй, лучший вариант
                      +3
                      определяйте порты в дейфайнах. Код не переносим.
                        0
                        Согласен. Я лишь привел пример того, как можно избавиться от полотна if'ов.
                    0
                    максимальная скважность получается 25%?
                      +1
                      >Примечательно, что оба аналоговых привода могли работать нормально только пару минут, а потом начинать произвольно дергаться, зажиматься в крайних положениях и т.д.

                      Вот это в высшей мере странно. Т.к. электроники внутри там почти нет. Тупейшая разностная схема которая зависит ТОЛЬКО от импульсов. А импульсы часом никуда не уплыли?

                      Бегло глянул на алгоритм — импульсы идут друг за другом, ширина периода каждого зависит от ширины соседей получается?
                        0
                        Сервоприводы специально ездил проверял в магазин — с сервотестером любые работают отлично, но с моей электроникой аналоговые дружить отказываются. Я не смог найти этому никого разумного объяснения.

                        Частота фиксированная и определяется переменной cycle (хотя ее надо было прописать как константу). У меня период следования был 18000мкс и четыре привода. Я записал в cycle 4500мкс — это время которое отводится на управление одним приводом, то есть передние фронты импульсов задержаны друг относительно друга именно на это время. Когда прерывание вызывается в первый раз — в OCR1A записывается длительность импульса управления приводом. При втором срабатывании, когда импульс заканчивается, в OCR1A записывается (cycle-angle1), время которое осталось до перехода к следующему приводу. В итоге частота следования импульсов не зависит от длительности управляющих импульсов.
                          0
                          Чудес не бывает. Берите осциллограф и сравнивайте сигнал сервотестера и своей схемы. Где то там косяк. Скорей всего джиттер.
                            0
                            У меня в распоряжении сервотестера к сожалению нет. Да и самой платы тоже. Примечательно, что на табло тестера угол выставляется в тех же единицах к которым я пришел, то есть от 800 до 2300, примерно. Это меня наводит на мысль, что я делаю все правильно.
                        +2
                        Ну, а из плюсов алгоритма. ОЧЕНЬ быстрая обработка прерывания. Это большой большой плюс.

                        Минус — плавающая частота импульсов. Не критично для положения, но критично для скорости вращения. Впрочем, тут не подразумевается разгон серв, так что влияние незначительное.
                          +1
                          Почему не подходит аппаратный ШИМ на контроллерах?
                            +2
                            потому что их обычно 1-3, а для, скажем, гексапода нужно 12-16
                              +1
                              у 16 максимальная скважность будет 100/16
                              тут лучше использовать внешний PWM, например, tlc5940
                            +3
                            Вставлю свои пять копеек.

                            За реализацию респект, уважение и поклон!

                            Но, критика ценнее и важнее похвалы. Тот кусок кода, который вы привели совершенно не переносим. Более того, если вы переставите сервопривод на другую ножку другого порта, вам придётся перепахивать всю программу.

                            Конкретно вот: PORTC |= 0b00000001; — это форменное безобразие и источник множества ошибок и проблем! Как это искоренить в коде я не представляю. Самое ужасное, что если вы решите перенести код на другой процессор, или о боже, другой архитектуры, то это будет весьма тривиальной задачей.

                            Я даже намедни писал пост по теме: dlinyj.livejournal.com/593356.html

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

                            #define		port_rele1	PORTA
                            #define		ddr_rele1	DDRA
                            #define		pin_rele1	PINA
                            #define		port_rele2	PORTC
                            #define		ddr_rele2	DDRC
                            #define		pin_rele2	PINC
                            ...
                            //pins define
                            //port_rele1
                            #define		K1	5
                            #define		K2	4
                            #define		K3	3
                            #define		K4	0
                            #define		K5	1
                            #define		K6	2
                            ...
                            


                            И далее обращался к ним вот так:

                            //инициализация
                            void hardware_init()
                            {
                            	ddr_rele1	|=(1<<K1)|(1<<K2)|(1<<K3)|(1<<K4)|(1<<K5)|(1<<K6);
                            	port_rele1	|=(1<<K1)|(1<<K2)|(1<<K3)|(1<<K4)|(1<<K5)|(1<<K6);
                            	ddr_rele2	|=(1<<K7)|(1<<K8)|(1<<K9);
                            ...
                            }
                            
                            //для включения, выключения пользовался макросами
                            
                            #define BIT_ON(port, bit)		do {port |= (1<<bit);}while(0) 
                            #define BIT_OFF(port, bit) 		do {port &=~(1<<bit);}while(0)
                            #define BIT_INVERCE(port, bit) 		do {port ^=(1<<bit);}while(0)
                            
                            //пример управления:
                            	BIT_OFF(port_rele1,K1);
                            
                            


                            Самое ценное, что поменяв только дефайны, я могу перенести данный код даже на другой процессор, если там другой тип обращения к пинам порта, то поменять мкакросы. Мне не нужно для этого будет перепахивать весь код.

                            Надеюсь комментарий будет полезен.
                              +2
                              Кодеры с работы тоже долго надо мной потешались. Просто у меня была очень конкретная разовая задача и я не уделил должного внимания улучшению читаемости кода. Кроме того здорово сроки поджимали — я перепробовал кучу вариантов управления и времени на финальную реализацию почти не осталось. Написал «чтобы работало». Следующим шагом будем думать над красотой кода. Спасибо за пример.
                                +1
                                Я так понимаю этот комментарий был адресован мне :).

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

                                Здесь нет места красоте. Поймите, что такой подход сокращает время разработки и уменьшает количество ошибок раз в десять!
                                  +4
                                  > Надо писать сразу читаемый код
                                  Ну у вас тоже неидеальный код.
                                  К примеру, стандарт де-факто — написание дефайнов только капсом.
                                  Или я так и не понял что за сумрачная конструкция do/while(0), её используют часто для того чтобы можно break юзать вместо goto, но в данном случае совсем мне непонятно.
                                  В дефайнах параметры не обернуты скобки:
                                  BIT_ON(BASE_PORT + PORT_STATUS, 2 * i) очень удивит компилятор.
                                  Ну и всякая мелочевка типа рандомной расставки пробелов.
                                    +1
                                    Ну и всякая мелочевка типа рандомной расставки пробелов.
                                    это не мой косяк, а косяк интрепретации хабром моего кода. У меня всё ок.

                                    К примеру, стандарт де-факто — написание дефайнов только капсом
                                    впервые слышу, быть может это конечно и оправданно.

                                    Или я так и не понял что за сумрачная конструкция do/while(0), её используют часто для того чтобы можно break юзать вместо goto, но в данном случае совсем мне непонятно.
                                    Это макросы Аскольда Волкова, на сколько я понимаю использование ду вайл делается для переносимости кода. Не разбирался.

                                    BIT_ON(BASE_PORT + PORT_STATUS, 2 * i) очень удивит компилятор.
                                    Силился представить, когда мне понадобится такая конструкция, но не смог… В особенности конструкция BASE_PORT + PORT_STATUS — не будет работать со сто процентной вероятностью. Но про скобки возможно замечание и стоящее.
                                      0
                                      > Это макросы Аскольда Волкова
                                      Вспомнил еще один вариант:
                                      if (XXX)
                                          SOME_MACRO();
                                      

                                      Если макрос состоит из нескольких выражений (#define SOME_MACRO bla(); bla();), то нужно обернуть в {}.
                                      Но в данном случае это не так (всего одно выражение), и разворачивать его в блок совсем не обязательно.
                                      > впервые слышу, быть может это конечно и оправданно.
                                      Я видел в нескольких code style стандартах.
                                      В том числе и гугловском.
                                      > Силился представить, когда мне понадобится такая конструкция, но не смог…
                                      Ну к примеру есть 4 порта — 2 набора команды/данные для 2х внешних устройств.
                                      Выясняем при конфигурации к какому набору подрублено устройство, и пишем в переменную base_port, а к самим портам уже обращаемся через base_port + PORT_DATA и base_port + PORT_COMMAND. В компьютерном железе такое не редкость. В МК думаю тоже бывает.
                                +1
                                А вот и видео разрушителя с краном:



                                и экскаватора:



                                как говорил раньше — кран все-равно вибрирует, но это уже связано с конструкцией.
                                  0
                                  Когда начал читать, думал все привода будут задействованы в одной модели на разные движения.
                                  ЗЫ. Плата у вас красивая получилась. ЛУТ?
                                    0
                                    Хотели изначально сделать чтобы клешня у разрушителя открывалась, но в итоге не получилось и двигается только три модели машин.
                                    Для изготовления плат давно уже использую ЛУТ.

                                    Only users with full accounts can post comments. Log in, please.