Адаптивное управление сервомашинкой. Часть 3.

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


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

— Читаем значение с АЦП
— Обрабатываем всеми нашими фильтрами
— Считаем управление
— Выдаем управление в ШИМ
— Остаток времени (меньше периода дискретизации)

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

— Выдаем управление в ШИМ
— Остаток времени (почти равен периоду дискретизации)
— Читаем значение с АЦП

Но тогда получается, что получив выход системы (значение с АЦП) надо как-то очень быстро посчитать управление и выдать его в ШИМ. Так быстро, чтобы остаток времени не сильно отличался от периода работы. Перетащим АЦП наверх, все же это цикл.

— Читаем значение с АЦП
— Выдаем управление в ШИМ
— Остаток времени (почти равен периоду дискретизации)

Так быстро посчитать во первых не получится, т.к. расчетов много а был выбран AVR. Во вторых, если бы и получилось, то это означало бы, что МК будет большую часть времени спать в ожидании завершения периода, а зачем использовать такой быстрый МК в задаче где он не требуется. Хотя где-то это могло быть примемлемо, но здесь нет.

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

— Читаем значение с АЦП
— Выдаем управление в ШИМ (по данным с прошлого такта)
— Расчет
— Остаток времени (меньше периода дискретизации)

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

% Exchange

	% -- ADC sample
	z = x(2) + randn(1) * nrms;

	% -- H-Bridge switch
	u = up;

	% Observer

	e = z - xl(2);
	xl = xl + 0.25 * [5e-2; 1] * e;

	xo(:,end+1) = [xl; z];

	% Identifier

	th = th + 100e-6 * [xl(1) u sign(xl(1)) 1] * e;

	% Prediction

	x1 = xl(1);
	xl = [th(1) 0; 1 1] * xl + [th(2); 0] * u ...
		+ [th(3); 0] * sign(xl(1)) + [th(4); 0];
	if ((abs(u) < 1e-3) && (x1*xl(1) < 0)) xl(1) = 0; end

	...


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

Если подумать, то подобным образом можно устранить любую дискретную задержку, не только на один такт. Только надо будет ещё и об устойчивости подумать.

Следующая проблема, как все это считать на AVR там же нет float? Софтовая реализация не считается, медленно оно слишком. В целых числах тоже сомнительно, будет много домножений и доделений на каждой операции, сложно контролировать правильность всех этих действий. Остается только фиксированная точка. Те же целые числа, но домножение/доделение будет упрятано в функцию умножения. Ничего хорошего готового найдено не было (смотрел на avrfix). Пришлось разбираться с тем как делать умножение больших чисел.

Оказалось все просто, но есть некоторые детали которых я не знал. Большие числа разбиваются на разряды-байты. В 32-битном числе на AVR 4 разряда. Байты из-за того, что инструкции mul* работают именно с ними, это минимальный кусок числа который можно перемножить аппаратно. Каждый разряд имеет свой вес, зависящий от того на сколько он удален от точки. Я использовал вот такие числа.

a b . c d


a b — старшие разряды хранящие целую часть
c d — младшие с дробной частью

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

a*255 + b + c/255 + d/255^2 


Тогда произведение двух чисел получается очевидным образом. В нем будет 4*4=16 умножений разрядов.

(a*255 + b + c/255 + d/255^2) * (e*255 + f + g/255 + h/255^2) =
 d*h/255^4 + c*h/255^3 + d*g/255^3 + ... + a*e*255^2


И это все верно для беззнаковых чисел. Но если старший бит в a и e содержит знак, то надо либо использовать muls/mulsu при их умножении либо вычитать из суммы ту добавку что туда вносят эти знаковые биты (об этом писано в [1]). И это как оказалось ещё не все. Пример. Где-то в середине этой суммы есть такое слагаемое, a*h*/255, один элемент знаковый другой беззнаковый, используем mulsu. Берем знаковый результат из r0/r1 складываем с регистрами аккумуляторами.


			"mulsu	r19, r20"	"\n\t"
			"add	r27, r0"	"\n\t"
			"adc	r28, r1"	"\n\t"
			"adc	r29, r24"	"\n\t"


К старшим регистрам надо добавить флаг переноса. Здесь есть ошибка. В r24 содержится ноль, но если результат умножения был отрицательный, то старшие биты надо дополнять не нулем а единицами. Как-то вот так. Возможно есть варианты получше.


			"mulsu	r19, r20"	"\n\t"
			"sbrc	r1, 7"		"\n\t"
			"com	r24"		"\n\t"
			"add	r27, r0"	"\n\t"
			"adc	r28, r1"	"\n\t"
			"adc	r29, r24"	"\n\t"
			"ldi	r24, 0"		"\n\t"


Полный код умножения прицеплен.

Испытания происходили на старой плате контроллера BLDC. Которая уже вся изжалена и перепаяна (сильнее чем на фото).



В схеме ничего интересно нет, H-мост (там и третий полумост есть, но не используется) управляется через порт C, без аппаратного ШИМ, вручную, сигнал с резистора идет на ADC вход.

Видео работы уже было.

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



В тиньках нет UART и умножения, а меги бывают мелкими только в BGA-образных корпусах. В TSSOP я ничего не нашел.

Ссылки.

[1] Генри Уоррен, Алгоритмические трюки для программистов.

Добавка

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

#define FABS(f)		((f) & 0x7FFFFFFFUL)


«slap your forehead and say d'oh!».jpeg
  • +5
  • 29 августа 2012, 15:06
  • amaora
  • 1
Файлы в топике: sci.tar.gz

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

RSS свернуть / развернуть
STM8S003F3. Мелкий, много ног, много начинки. Хотя мега8 в MLF тоже довольно компактна.
+1
  • avatar
  • Vga
  • 29 августа 2012, 16:13
Для них gcc нет, да и лень новые системы комманд, перифирию, программаторы, и т.д. изучать. Выбираю только из avr и stm32.
0
Тогда ищи мегу в MLF. Еще мона поискать MSP430 в TSSOP, скажем G2553 — там есть и UART (точнее, USCI), и АЦП, и 16кб памяти, и GCC для них есть.
Но меня бы такие мелочи не остановили) Тем более программатор для STM8 достаточно дешев, а бесплатные компиляторы имеются.
Ну и зачем изучать систему команд, если использовать С? Да и обычно нечего там особо изучать, таблички команд хватает.
0
Только зарегистрированные и авторизованные пользователи могут оставлять комментарии.