Управляем сервомашинкой из LabView

Предисловие.
Еще прошлым летом побывал я в Киеве, в магазине радио управляемых моделей. И там удалось мне купить потрепанную сервомашинку – PROTECH B-150. Вроде как механика у ней барахлила под нагрузкой, от того и списали. Но вообще рабочая, для экспериментов пригодная.
Наконец освоил управление ею. Осваивал постепенно, вначале просто при помощи JTAG отладчика менял длительность управляющего сигнала, потом переменный резистор и АЦП использовал, потом через терминальную программу слал требуемое положение, и наконец прикрутил LabView. Постараюсь все этапы изложить здесь: промежуточные вкратце, а всё что касается LabView — более подробно.

Этап 1.
Собираем схему.

Вот собственно сервомашинка.
Сервомашинка
Назначение проводов:
Коричневый – земля
Красный – «+» питания (рекомендуется 4,8В я использовал 5В)
Оранжевый – управляющий сигнал.
Для того чтобы вал сервомашинки принял нужное положение необходимо слать на сигнальный провод прямоугольные импульсы с длительностью от 0,9 до 2,1 мс. Между восходящими фронтами рекомендуется пауза 20мс.
DI HALT подробно описывает устройство и работу с сервой в своей статье Сервомашинка HS-311
В качестве посредника между ПК и машинкой использован контроллер ATMega16.
ATMega16 со схемой питания
В ходе учений задействуется одна ножка в качестве управляющей линии, и один канал АЦП, приёмо-передатчик UART.
Внутрисхемная прошивка и отладка велась посредством JTAG ICE отладчика сборка которого выполнена по материалам статьи.
Для связи с ПК использован преобразователь уровней, рецепт сборки которого взят от сюда.
Преобразователь уровней на MAX232

Вроде бы простая штучка, но пока я додумался поменять местами RX и TX дорожки идущие к COM порту бедная микросхема MAX232 отжигала очень серьёзно (буквально обжигала пальцы и при этом не работала), так что будьте внимательней, не копируйте бездумно чужие изделия.
Вот полная схема «экспериментального стенда».
Схема стенда
Электроника и сервомашинка запитаны от разных источников питания.
На всякий случай я организовал ключ на N канальном MOSFETе, и установил резистор 10 Ом для ограничения управляющего тока.
Работает всё это следующим образом: лапа контроллера (PORTB0) должна прижимать затвор транзистора к питанию чтобы управляющий провод сервомашинки ложился на землю. И наоборот – когда нога отпускает затвор транзистора, тот подтягивается к земле резистором 10кОм и транзистор прекращает сливать на землю положительный заряд – через резистор 1кОм на управляющем проводе устанавливается положительный потенциал.
А вот так стенд раскинулся на табуретках:
Стенд

Этап 2.
Формируем базовый алгоритм генерации импульсов.

Сперва настроим тактирование контроллера на работу с частотой 8МГц. Используя JTAG ICE отладчик, достаточно выбрать необходимую частоту из списка, шаманить с битами он будет сам.Подробнее смотрите в статье про JTAG, ссылка приведена выше.
Сконфигурируем порты для работы:

// ПОРТ А             76543210
	OUTI DDRA,  0b00000000    //Направление
	OUTI PORTA, 0b11111111    //Состояние
// ПОРТ В             76543210
	OUTI DDRB,  0b00000001    //Направление
	OUTI PORTB, 0b11111110    //Состояние
// ПОРТ С             76543210
	OUTI DDRC,  0b00000000    //Направление
	OUTI PORTC, 0b11111111    //Состояние
// ПОРТ D             76543210
	OUTI DDRD,  0b00000000    //Направление
	OUTI PORTD, 0b11111111    //Состояние

Чтобы дергать ногой используем прерывания по переполнению таймера 0. Инициализируем его таким образом:

IN R16,   TIMSK  //установка только нужного бита
ORI R16,  0b00000001 //Регистр маски прерывания от таймеров 
OUT TIMSK, R16
OUTI TCCR0, 0b00000001  //Регистр управления таймером 0

Файл проекта прикреплен к посту, там вы найдете инициализацию с более чем подробными комментариями.
Немного математики. Процессор тикает 8 000 000раз в секунду. Таймер инкерментирует свой счётчик с той же частотой.
Он переполняется с частотой: 8 000 000/256 = 31250 раза в секунду. Чтобы было ясней — тридцать одну тысячу раз с небольшим за одну секунду
Нам нужно отсчитать самое малое 0,9 мс. Чтобы узнать сколько это в переполнениях таймера составляем пропорцию:
1000мс =31250перполнений
0,9мс = (0,9*31250)/1000=28 переполнений
Для получения 2-х мс соответственно (2*31250)/1000=62 переполнения
И для получения 20-ти милисекундной паузы между восходящими фронтами сигнала:
(20*31250)/1000=627. Для упрощения отсчёта я использовал 254 переполнения, работает нормально.
Таймер мы запустили, не забудем перед главным циклом программы (он у меня получился совсем пустой) дать общее разрешение прерываний.

  SEI //Прерывания разрешены
MAIN: //Главный цикл программы
  NOP
  NOP
  JMP MAIN

В таблице прерываний дадим вектор таймеру 0:

.ORG	OVF0addr	// Timer/Counter0 Overflow
JMP Timer0Overflow

Напишем обработчик прерывания:

Timer0Overflow:          //Переполнение таймера 0
		INC FreqensyCounter  // увеличить счётчик полного периода управления
		CPI FreqensyCounter, FreqensyCounterSet //если период достигнут - 
		BREQ RestartControl  //начать новый цикл
                IN R16, PINB         // иначе проверить: бит в порту сброшен?
		CLC 
		ROR R16
		BRCS Timer0OverflowEnd  //Если нет тогда завершаем прерывание
		INC  PulsMesure       //если да тогда еще и увеличить счётчик длительности импульса
		CP  PulsMesure, PulsMesureSet //если длительность совпала с заданной -
		BRNE Timer0OverflowEnd        
                SBI  PORTB, Servo        //установить бит в порту
		RJMP Timer0OverflowEnd

RestartControl:    //новый управляющий цикл начинается
		CLR FreqensyCounter//обнулить счтчик полной длительности
		CLR PulsMesure     //обнулить счётчик длительности импульса
		CBI PORTB, Servo   //сбросить бит в порту

Timer0OverflowEnd:
RETI

Комментариев по моему достаточно, но всё же уточню, что таймер щёлкает двумя счётчиками: один отмеряет длительность управляющего сигнала (PulsMesure), второй меряет длительность паузы между восходящими фронтами (FreqensyCounter). Оба сравниваются с регистрами хранящими заданные значения (FreqensyCounterSet) и (PulsMesureSet) соответственно. Последнее следует задать в программе явно, в диапазоне от 28 до 62-х в но потом будем его менять програмно. Регистру FreqensyCounterSet просто присвоим значение 254.

.equ    PulsMesureSet =31
.equ    FreqensyCounterSet =254     

Когда совпадает счётчик длительности импульса (PulsMesure) – выставляем бит в порту и тем самым открываем транзистор, ложим сигнальную линию сервы на землю. Импульс заданной длинны передан.
Если совпадает значение полного периода (FreqensyCounter) – начинаем новый управляющий импульс: сбрасываем бит в порту, обнуляем оба счётчика и снова ждём прерывания.
Итак, порт настроен на выход, тактовый генератор сконфигурирован, таймер настроен и запущен, прерывания разрешены, обработчик написан.
Собираем схему, выходим на связь с контроллером из AVR студии по при помощи JTAG, заливаем прошивку, жмем на Play. Если всё правильно, то серва повернется на некоторый угол и останется в этом положении. Изменим текст программы, поменяв значение PulsMesureSet в рабочих пределах (28-62)снова откомпилируем программу и прошьём в контроллер — сервомашинка вертит хвостом!
Хорошо, сервозверек подает признаки жизни(очень надеюсь что таки да). Сделаем его дрессировку чуть более удобной.

Этап 3.
Реальная крутилка для реальной вращалки.

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

OUTI ADMUX,    0b00100000
BYTEMOD SFIOR, 0b00011111, 0b10011111  
OUTI ADCSRA,   0b11101110

Все коментарии в окно не влезут, я прикреплю файл с проектом к посту, там всё посмотрите.
Чтобы перезапускать преобразования, используем прерывание таймера Т0. Там ничего не надо дописывать, мы всё указали в регистрах конфигурации АЦП.
Поскольку данный этап изучения лишь промежуточный, я не задавался целю обеспечения плавного вращения сервомашинки. В связи с этим, алгоритм обработчика прерывания от АЦП выдает лишь 8 значений PulsMesureSet, в зависимости от того в какой диапазон значений попал старший байт выходного слова АЦП.

ADC_Conversion_Complete: //Преобразование АЦП завершено
		IN R16, ADCL
		IN R16, ADCH //забрали результат
		
		//Таблица поиска диапазона
		CPI R16, 32
		BRLO Step_0
	        CPI R16,    64
		BRLO Step_1
		CPI R16,    96
		BRLO Step_2
		CPI R16,    128
		BRLO Step_3
		CPI R16,    160
		BRLO Step_4
		CPI R16,    192
		BRLO Step_5
		CPI R16,    224
		BRLO Step_6
		LDI PulsMesureSet, 62
		RJMP  ADC_Conversion_Complete_End
		

	Step_0:
		LDI PulsMesureSet, 28
		RJMP  ADC_Conversion_Complete_End
    Step_1:
		LDI PulsMesureSet, 32
		RJMP  ADC_Conversion_Complete_End
    Step_2:
		LDI PulsMesureSet, 36
		RJMP  ADC_Conversion_Complete_End
    Step_3:
		LDI PulsMesureSet, 40
		RJMP  ADC_Conversion_Complete_End
    Step_4:
		LDI PulsMesureSet, 44
		RJMP  ADC_Conversion_Complete_End
    Step_5:
		LDI PulsMesureSet, 48
		RJMP  ADC_Conversion_Complete_End
    Step_6:
		LDI PulsMesureSet, 52
	

ADC_Conversion_Complete_End:
RETI 

Не забудьте закоментировать или удалить строку, где прописано значение регистра PulsMesureSet, вот так:

//.equ    PulsMesureSet =31

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

Этап 4.
На связи с ПК.

Как видите, чтобы менять положение вала сервомашинки достаточно менять значение одного байта. Ничего не мешает нам обновлять этот байт используя UART.
Для этого понадобится ПК в котором есть COM порт (у меня стоит плата расширения на 2 порта – не пожалел что купил), преобразователь интерфейсов, и программа-терминал которая умеет принимать/передавать данные через COM порт. Ознакомится с некоторыми из них можно здесь. Я использую Terminal v1.9b by Bray.
Правда есть одна сложность: терминальные программы (и LabView) передают строки в ASCI кодировке. Так что если вы захотите отправить число 23 то оно отправится в идее двух символов с кодами 50 и 51. А нам то нужно 23! Чтобы понять, как перекодировать строку обратно в число посмотрим на таблицу ASCII кодов для цифр:
ASCII код Символ.
048 0
049 1
050 2
051 3
052 4
053 5
054 6
055 7
056 8
057 9
Вот получили мы, к примеру код 54 (это символ 6), если вычесть из него код символа 0 (48) то получим 6 – просто число готовое к дальнейшей обработке.
А дальше всё просто: мы точно знаем, что передавать будем значения от 28 до 62 (всегда две цифры. К нам они будут приходить в виде двух символов, старшим байтом вперед. Каждый символ расшифруем в простое число и сформируем нашу переменную PulsMesureSet: байт с десятками умножим на 10 и прибавим байт единиц.
Инициализируем USART

OUTI UCSRA,      0b00000000 //можно было и не писать, но всё же... 
OUTI UCSRB,      0b11011000
OUTI UCSRC,      0b10000110
OUTI UBRRL, low(bauddivider)  //загрузка делителя частоты
OUTI UBRRH, high(bauddivider) 
//инициалаизация буфера приёма
LDI XL, low(BuferStart)
LDI XH, high(BuferStart)
LDI MesageSize, 0

Напишем обработчик прерываний:

USART_RX_Complete: //приём окончен

		IN R16, UDR
		ST X+, R16        //запись в буфер приёма
		INC MesageSize    //увеличение счётчика полученых байт
		CPI MesageSize, 2 //если получены не все байты
		BRNE USART_RX_Complete_End //завершить прерывание
		//иначе обработать посылку
                LD   R16, -X   //берем из ОЗУ младший байт
		NOP
		CLC //сброс флага переноса
                CLH //сброс флага половинного переноса
		CLZ
		SUBI R16, 48 //Вычитаем код нуля, получаем количество единиц
		LD   R17, -X   // берем из ОЗУ старший байт
		NOP
                SUBI R17, 48 //Вычитаем код нуля, получаем количество десятков
                LDI PulsMesureSet, 10 //узнаем сколько десятков
		MUL PulsMesureSet, R17 //получили десятки
		MOVW PulsMesureSet, R0 //забрали младший байт результата
		ADD PulsMesureSet, R16 //всё, посылка принята.
		//переинициализируем буфер приёма
		LDI XL, low(BuferStart)
		LDI XH, high(BuferStart)
		LDI MesageSize, 0

USART_RX_Complete_End:
RETI

Не забудьте закоментировать инициализацию АЦП, чтобы он не совал в PulsMesureSet свои значения.
Прошивку заливаем в контроллер, запускаем терминальную программу, указываем используемый порт и скорость передачи (9600), отправляем строку со значением в рабочих пределах(28-62) – смотрим, на реакцию сервомашинки.
Этап 5.
Виртуальная крутилка для реальной вращалки.

Терминал это конечно удобно, но хочется еще удобней. Задействуем LabView. Чтобы не дублировать информацию я настоятельно рекомендую ознакомится с вот этой статьей. Из неё вы получите некоторые базовые приёмы работы, узнаете как установить программу VISA, и использовать СОM порт.
Цель нашей программы следующая: вслед за вращением органа управления (объект Dial) должен поворачиваться вал сервомашинки.
Как помнится, контроллер умеет принимать и преобразовывать строку из двух символов, которая приходит из ком порта. Значит наша программка должна слать в COM порт новое значение как только оно появится на выходе крутилки-Knob.
Создаем новый пустой виртуальный прибор. В окно рабочей панели добавляем объект Dial.(На панели Controls ->Modern -> Numeric ->Dial).
Его нужно настроить, так чтобы он выдавал значения нашего рабочего диапазона с шагом 1, да и размерчик сделаем покрупнее. Для этого кликнем правой кнопкой мыши на объекте — появилось контекстное меню. Жмем самый нижний пункт — Properies. Настраиваем диапазон работы в закладках так как показано на рисунке.



Чтобы увеличить размер объекта наедите на него мышь — появятся синие маркеры. За них можно растащить морду этому вернеру как душа пожелает.
Добавьте на рабочую панель объект StringIndicator. Modern -> String&Pass -> StringIndicator. Он нам понадобится для просмотра промежуточного результата работы программы.
У меня получилась такая вот морда прибора (откуда взялась кнопка Стоп, и селекторы порта объясню чуть ниже.)

Теперь пришло время нарисовать программу. В главном меню LabView жмем пункт Window -> Show Block Diagram.
В рабочем пространстве алгоритма уже есть наши Dial и StringIndicator.
Сначала сделаем программную обвязку для Dial. Она должна обеспечивать выдачу в главный цикл только НОВЫХ значений длинны импульса. Иначе программа будет непрерывно передавать контроллеру одно и тоже значение, отвлекая его на прерывания из которых он ничего нового для себя не узнает. Посмотрите что должно получится, ниже я опишу что это, и как его построить.

Вначале рисуем рамочку структуры WhileLoop [1] (Панель функций ->Programming->Stuctures->WhileLoop). Всё что находится внутри этой рамочки будет циклически повторятся до тех пор пока в объект LoopCondition [9] не получит на свой вход булевое значение «True». (можно указать на что он среагирует правда/ложь вызвав его контекстное и указать вариант Stop If True / Continue If True).
Чтобы можно было остановить данный цикл (а потом и всю программу) создадим кнопку стоп, тут же в окне редактирования алгоритма: контекстное меню на LoopCondition [9] -> CreateControl. Появившуюся кнопку оттащим влево, и временно разорвем её связь: щелчок на проводе и клавиша Delete.
Внесем наш вернер (Dial) [2] в цикл. Добавим и разместим следующие объекты:
Компаратор «Не равно» [3]. Выдает True когда входящие операнды не равны друг другу. Programming->Comparison->NotEqual.
Рамочку структуры CaseStructure [4]. Выполняет тот или иной кусок кода в зависимости от варианта пришедшего на вход с зелёным знаком вопроса. В простейшем случае вариантов всего 2 — True u Fasle. Programming->Stuctures->CaseStructures
Булевое «ИЛИ» [8]. Если получает на один из своих входов True, то и на выход выдаст True. Programming->Boolean->Or.
Теперь будем постепенно связывать всё хозяйство:
1)Соединяем Dial [2] и один из входов компаратора [3].
2)Результат работы компаратора направим в CaseSelector нашей CaseStructure [4]
3)Подадим значение Dial на вход Case структуры. Тянем проводочек к краю рамочки и жмем кнопку мыши. Получившийся квадратик — это туннель. Он пропускает сквозь себя данные в конце каждой итерации программы находящейся внутри рамки.
4)Опишем случай True. Установим нужное состояние case-структуры при помощи стрелочек < > в заглавии рамки. Протянем проводочки от входного туннеля и создадим два исходящих.
Верхний туннель соединим проводочком с рамкой цикла [1]. Получился туннель, но нам нужен сдвиговый регистр. Вызываем контекстное меню на туннеле и выбираем Replase With Shift Register. Получился сдвиговый регистр [7]. Он будет сохранять в себя результат текущей итерации и передавать его на вход следующей. Теперь можно подать второй операнд на вход компаратора [3] из сдвигового регистра. Нижний туннель просто соединяем с рамкой цикла 1.
5)Вариант False изображен нижнем рисунке — полный алгоритм. Как видите, нижний выходной туннель не подключен. Чтобы программа корректно работала, следует вызвать на нём контекстное меню и дать команду «Use Defult If Unwired».
6)Теперь соединим кнопку Стоп, компаратор и элемент ИЛИ [8], результат подадим на LoopCondition [9].
7)Выведем проводок от кнопки Стоп из цикла через туннель.
Далее следует описать главный цикл программы.

Обрамляем всё ранее созданное художество циклом WhileLoop. Подсоединяем LoopCondition внешнего цикла к кнопке Стоп через ранее созданный туннель.
Как правильно добавить объекты для работы с ком портом описано сдесь. Но нам нужно не принимать, а передавать байты, поэтому вместо блока Read используем блок Write. Instrument I/O->VISA->Visa Advanced->BusSpecific->Serial->Visa Write.
Этот блок питается стрингами, то есть строками. Поэтому добавляем объект Number To Decimal String. Он получает на вход число, а на выходе отдает последовательность ASCII кодов десятичных цифр. Programming->String->String/Number Convertion->Number To Decimal String.
Соединяем выходной туннель цикла [1] c преобразователем, а от него передаем строку передатчику и индикатору.

Всё. Построение алгоритма завершено!
Перейдите на панель управления и разместите новые элементы: кнопку Стоп, поле выбора порта, и задания скорости обмена.
Как это работает?
Лучше всего и наглядней будет сделать так: в окне редактирования алгоритма нажмите кнопку с лампочкой. (Пятая в верхнем меню).
Теперь жмите кнопку со стрелкой «Run». Программа сама будет показывать в какой последовательности выполняются операции и каковы их промежуточные результаты.
Хочу отметить важный нюанс: блоки LabView выполняются только тогда когда получили на свои входы все необходимые данные.
Именно этой особенностью мы и пользуемся: если вернер не повернулся, его текущее (старое) значение не выпускается из итерации цикла, преобразователь Цифра/ASCII не получает нового задания, не выдает новой строки — передатчику нечем питаться посему он не передаёт. Но как только вернер изменил свое положение, цикл его опроса останавливается благодаря компаратору сравнишему новое и старое из сдвигового регистра, и новое значение летит в преобразователь, а от туда строка мчится в передатчик и в наш контроллер.
Схема уже собрана. Остаётся только указать в панели управления виртуального прибора задать номер порта и скорость.
Жмем на кнопку Run и смотрим на результаты наших трудов.

Архив с проектом AVR
Програмка в LabView
Очень многА букаФ получилось :(
Жду комментариев и замечаний.
  • +1
  • 26 июня 2011, 01:11
  • mwandry
  • 3
Файлы в топике: Servo_Protech_150.JPG, RS232T.zip, Servo_Drive.zip

Комментарии (19)

RSS свернуть / развернуть
Он переполняется с частотой: 8 000 000/255 = 31372 раза в секунду.
8000000/256 на самом деле. Это 31250 Гц.
0
  • avatar
  • Vga
  • 26 июня 2011, 01:59
Виноват, уже исправил.
0
Хошь сказать у меня ошибка в печатке?
0
Получается что так. На фото видно (хот и паршиво). Пришлось перерезать дорожку и сделать новые связи проволочками.
0
думать надо было головой, а не тупо припаивать. Я для кого там на плате номера пинов нарисовал? Вот соответственно им и надо было действовать. А для пайки напрямую к разьему эта разводка и не годится. ВО первых расстояние до пятаков совершенно другое (то что ты ее умело вкорячил ничего не значит, хотя видно же что не совпадает нихрена). Во вторых особого смысла нет, т.к. в корпус разьема дип микруха один хрен не влезет.
0
Про эту ошибку(печатка для дип-версии) ЕМНИП еще в коментах к самой статье писали
0
А не надо ее припаивать к плате напрямую разьемом. Сей финт только для SMD версии. Дип версия для этого не предназначена. Там для чего номера контактов на плате вытравливаются? Вот по ним и надо было проводами до разьема делать. Там же даже пятаки стоят на другом расстоянии.
0
Проще немного подумать и переразвести пару дорожек, перед тем как травить. Или после травления, если не получилось подумать, перерезать пару дорожек и перемычек накидать :)

А номера контактов у меня не вытравливались и не должны :) Я маркером рисую
0
Дык дип-вариант и трассировать стоило под дип-разъем. В смысле, он не дип конечно, но суть ясна думаю :)
0
хорошая статья! Спасибо
0
Хочу еще заметить, что подсветка выполнения кода в LabView некисло отжирает ресурсы ПК и обычно программы подтормаживают, хотя это зависит от железа.
0
Пожалуйста. Рад что кому то пригодилось.
0
Что будет, если подать питание на сервомашинку без подачи управляющего сигнала?
Если сервомашинка и мк питаются от 5 Вольт, зачем подавать управляющий сагнал через транзистор а не напрямую?
0
  • avatar
  • tank
  • 03 августа 2011, 09:21
По первому пункту экспериментов не ставил, придумывать тут не буду.
По поводу второго соображения следующие: можно напрямую подключать, если заранее известно, что входное сопротивление машинки достаточно большое чтобы ток управляющего импульса не превысил 100мА.(иначе скопытится порт, или весь МК)
Скорее всего так оно и есть, но всё же я решил подстарховаться.
0
Чтобы защитить порт мк, я бы поставил резистор на 100 Ом :)
0
Ей параллельно. Пока не придет импульс, машинка не почешется даже встать в нейтраль.
0
Что за параметр такой: «Dead Band Width: 4usec»?
— у сервомашинки SG90
0
  • avatar
  • tank
  • 05 августа 2011, 12:48
Мертвая зона. Если длительность импульса отличается от соответствующей текущему положению менее чем на это значение — машинка проигнорирует это различие. Нужно для того, чтобы машинка останавливалась в нужном положении, а не дрыгалась вокруг него.
0
Попадалась машинка с 7мкс Dead Band Width.
Где оптимум?
0
Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.