Измерение скорости мотора на Arduino
Методист по олимпиадной робототехнике Университета Иннополис Алексей Овсянников продолжает серию уроков.
Продолжаем разговор об Arduino и управлении моторами с ее помощью. Новые статьи не выходили уже несколько дней, пришлось долго собирать материал, тестировать программы и суммировать результат. Статья получилась длинная, но надеюсь, что интересная.
В прошлых статьях я рассказывал, как с помощью прерываний отслеживать показания энкодеров, они нам расскажут о пройденном роботом расстоянии. Сегодня хотелось бы рассмотреть способы измерения скоростей моторов с помощью Arduino.
Давайте подумаем…
… о том, как нам это можно сделать. Скорость есть изменение положения за единицу времени. Другими словами — как быстро меняется положение объекта (робота) в пространстве или как быстро меняется положение вала. Математически это записывается как
где:
V — линейная скорость;
⍵ — угловая скорость;
S — путь, пройденный объектом (роботом), вычисляется как S=Pнов-Pстар (P — положения новое и старое, разница/расстояние между ними должна быть указана в метрах);
T — время измерения, начинается, когда объект (робот) находится в Pстар и заканчивается, когда объект находится в Pнов;
— угол, на который повернулся вал мотора ( — углы/положения вала новое и старое, разница между ними должна быть указана в градусах).
Очень часто эти формул пишут в следующем виде:
В них dP — изменение положения, dE — изменение угла, dt — «изменение времени» — время, за которое произошли эти изменения угла или положения.
Первое выражение — для линейной скорости, второе — для угловой. Связать линейную и угловую скорости можно через диаметр колес и число Пи. Для простоты в сегодняшнем примере будем определять только угловую скорость.
Для вычисления скорости нам надо уметь запоминать и сравнивать положения робота в разные моменты и находить, сколько времени прошло между этими моментами. Ну и удобнее всего сделать так, чтобы эти измерения и вычисления проходили в фоне, а в основной программе мы видели актуальные значения скоростей.
На Arduino фоном могут происходить прерывания. В прошлых статьях я рассказывал о прерываниях от внешних сигналов (они так и называются «внешние прерывания»), но существуют еще прерывания по таймерам, прерывания от интерфейсов связи (срабатывают, например, при получении сообщения по UART/Serial или I2C) и многие другие.
Прерывания по таймеру нам могут подойти. Работают они следующим образом — в микроконтроллере независимо от основной программы работает таймер, когда он отсчитывает нужное время — срабатывает прерывание, основной скетч останавливается и вызывается подпрограмма — обработчик прерывания. В этот же момент таймер сбрасывается и начинает считать заново. После завершения обработчика выполнение возвращается в основной цикл, в то место, где он был прерван. Таким образом таймер не зависит от того, как долго или быстро выполнялся обработчик прерывания, все работает на аппаратном уровне. То есть, при использовании прерываний по таймерам T будет постоянной, заданной заранее в настройках таймера. Нам каждый раз прерывания таймера надо будет запоминать положение вала и сравнивать его с предыдущим.
Вот тут стоит остановиться и сделать ОЧЕНЬ важное замечание — инструментарий и среда Arduino не предоставляет нормального способа работы с таймерами. Можно, конечно, работать с регистрами самого микроконтроллера, но в разных платах они разные (AVR, SAMD21 и т.д.), и регистры у них разные. Кроме того, у каждой платы на таймерах висит куча важных и полезных функций: генерация ШИМ на ножках, работа по Serial’у, функции millis() и micros() и многие другие. Так что новичкам их вообще лучше не трогать, а мы для эксперимента попробуем, но очень аккуратно! Так что перед их использованием….
Давайте почитаем…
… даташиты к микроконтроллерам. Хорошо, не пугайтесь, официальный даташит на 300 страниц сейчас читать не будем, это история для сильных духом. Мы немного схитрим и почитаем только нужный минимум. В первую очередь надо определиться с платой и используемым в нем микроконтроллером. Я для тестов использовал плату Iskra Neo фирмы Амперка, она построена на базе микроконтроллера ATmega32u4 и полностью повторяет оригинальную Arduino Leonardo. Можно было взять Arduino Mega 2560, но в ней есть несколько свободных таймеров, не наглядны будут следующие шаги.
Если мы просто загуглим «Arduino timers interrupt» или «Arduino timer прерывания», то с большой вероятностью найдем вот эту статью на Хабре. Вроде бы хорошая популярная библиотека, но в описании видим:
То есть, для нашего МК библиотека предоставляет доступ только к таймеру 1, а на нем как раз «висят» многие важные функции, в том числе Serial. Еще раз напоминаю, что «мы ходим по очень тонкому льду» и не стоит трогать этот таймер без необходимости.
Гуглим дальше, например «Arduino Leonardo timers» и натыкаемся на другую хорошую статью. В ней присутствует важная таблица:
Так-так, уже интересно. То есть, на таймер 3 завязан только ШИМ на пине 5, на таймере 4 ШИМ сигнал на пинах 6 и 13. Я планирую брать скорость мотора с пина 6, поэтому перенастраивать таймер 4 не стоит. Придется работать с таймером 3, отказавшись от ШИМа на 5 ножке Arduino Leonardo / Iskra Neo. Обратите внимание, что если бы для управления моторами использовался Amperka Motor Shield, то такое решение не подошло бы — он использует ШИМ сигнал с пинов 5 и 6; пришлось бы выбирать другой таймер или перенастраивать шилд на работу с другими пинами.
Пока фиксируем промежуточный этап — работать будем с TIMER3.
Работать напрямую с регистрами самого МК для настраивания таймера ой как не хочется (как это делается можете посмотреть в этой статье на Хабре), поэтому продолжаем гуглить: «Arduino leonardo timer3». Находим хорошую статью, в которой описываются библиотеки для работы с TIMER1 и TIMER3. Они позволяют настроить период таймера и прерывания, то, что нам и нужно. Вот ими и будем пользоваться.
Давайте посчитаем…
… нужную частоту таймера, точность измерений и другие параметры.
Все тесты я провожу на следующей аппаратной части:
- Контроллер Iskra Neo от фирмы Амперка;
- Двигатель постоянного тока Pololu 25mm 12V medium power 100 RPM;
- Драйвер для двигателя Pololu md08a;
- Лабораторный блок питания, для подачи питания номинального напряжения 12В.
От них и будем отталкиваться при расчетах. У энкодеров будем считывать «тики», а не состояния (на простых контроллерах Arduino с ограниченным количеством прерываний такой случай более актуален, посчитаю и покажу именно его).
Чтобы всегда иметь актуальные скорости, их надо вычислять как можно чаще. В идеале — на каждом тике энкодера. То есть, при новом «тике» энкодера вызывается обработчик прерывания, в нем сравниваем текущее время с временем предыдущего «тика» и вычисляем dt. Для ускорения вычислений ограничим переменную для dt двумя байтами и целым числом, так как операции с ними выполняются быстрее, чем операции над дробными числами или над четырехбайтовыми переменными (например, long). Время может только увеличиваться, то есть переменная для dt должна хранить только положительные числа. Для этого идеально подходит тип word, который хранит числа от 0 до 65535. Для увеличения точности время будем измерять в микросекундах с помощью функции micros(). При остановке мотора (или очень малых скоростях) новый «тик» может не происходить очень долго, и переменная переполнится.
Итого, для адекватного запоминания времени между тиками они должны происходить не реже, чем каждые 65535 мкс = 65,535 мс = 0,065535 с. Если энкодер выдает импульсы реже, то переменная dt будет переполняться. Я в программе буду это отслеживать и принудительно вписывать в нее максимальное значение. Таким образом минимально вычисляемая угловая скорость вала энкодера:
Так как за один оборот вала энкодер делает 12 тиков, то один тик равен 360/12 = 30 градусов. Переводим тики в градусы и получаем
У выбранного мотора Pololu передаточное отношение редуктора равно 74,83. То есть, выходной вал крутится в 74,83 раза медленнее, чем вал энкодера, и мы получим
Номинальная скорость мотора на холостом ходу составляет 100 об/мин или 600 град/сек. Диапазон измерений от 6 до 600 град/сек считаю нормальным, недоступным останется примерно 1% от всего диапазона.
Второй способ, который мы рассматриваем, заключается в измерении скорости по таймеру. Опытные Lego-робототехники стараются вычислять новые управляющие воздействия регуляторов каждые 1-5 мс. Выбранный нами мотор на номинальной скорости 100 об/мин делает один тик за 0,641 мс. За 5 мс энкодер наматывает менее 8 тиков (5 мс/0,641мс = 7,8). Получается, возможны только восемь значений вычисленных скоростей для dE=0…7. Точность так себе. Увеличив период таймера (и вычислений) до 10 мс получим уже до 15 тиков энкодера на максимальной скорости, с периодом таймера 50 мс получим 78 тиков (точность измерения скорости примерно 1,3%). Увы, увеличивая дальше период таймера мы сильно снижаем частоту вычисления новой скорости, это уже сильно скажется на точности движения с синхронизацией моторов по скорости. Даже с периодом 50 мс контроллер будет успевать подстраивать их скорости всего 20 раз за секунду. Во время тестирования я специально изменю периоды и покажу на графиках, как от этого ухудшаются показания скорости.
Давайте попрограммируем…
… все, что раньше посчитали.
Для начала установим и подключим библиотеку TimerThree. Библиотеки можно качать и устанавливать архивом с сайта разработчика или искать во внутреннем «магазине» библиотек. Попробуем второй метод, для этого в Arduino IDE переходим в меню «Скетч — Подключить библиотеку — Управлять библиотеками»
В появившемся менеджере библиотек вводим название интересующей нас «TimerThree», находим ее в списке и устанавливаем:
Теперь библиотеку можно подключить через меню или прописав в начале файла
#include «название_библиотеки.h»
Теперь можно писать код. Начнем с метода измерений в обработчиках внешних прерываний. В прошлой статье я приводил код обработчика:
Надо его дополнить вычислением времени. Сравниваем текущее время с временем предыдущего прерывания, запоминаем текущее время в переменную oldTime для следующего раза. Как уже говорилось ранее, надо проверить, не прошло ли с прошлого тика слишком много времени (которое переполнит переменную dt типа word) и при необходимости заполнить ее руками.
В своем примере я сразу в обработчике прерывания вычисляю угловую скорость w_interrupt. Операции деления, работа с дробными числами выполняется микроконтроллером довольно долго, поэтому лучше было бы их вынести из прерывания (чтобы оно выполнялось максимально быстро). Достаточно оставить в нем вычисление dt и dF, а само значение угловой скорости считать по необходимости в основной программе, используя эти значения.
В этом месте стоит остановиться и обратить внимание на два важных момента:
- Все переменные, используемые в прерываниях и общей программе, должны быть объявлены как volatile, иначе компилятор для экономии ресурсов контроллера может их заменить на заранее выбранные значения. Ключевое слово volatile подскажет компилятору, что переменную нельзя заменять, а надо работать с ней именно как с переменной.
- В самом начале обработчика прерывания я создаю переменную thisTime и кладу в нее текущее значение micros(), далее работая уже с переменной. Команда micros() вызывается всего один раз за прерывание. В описании команды attachInterrupt() есть пометка, что при повторном вызове micros() через некоторое время она может вести себя непредсказуемо.
Теперь можно начинать работать с прерываниями по таймеру. Подключаем библиотеку, устанавливаем период таймера и подключаем к нему обработчик прерывания (часть кода удалена для наглядности):
Здесь удален код, отслеживающий показания энкодера по обычным внешним прерываниям. Они считаются в фоне и доступны в переменной enc.
Полный код программы доступен на гитхабе.
Давайте проверим…
… как это все работает. В основном цикле я буду управлять мощностью мотора с помощью ШИМ-сигнала. Для наглядности я выведу показания скоростей в виде графиков. Кроме того, я добавлю вычисленную «возможную» скорость. Вычислять буду примитивно линейно: если на номинальном напряжении (analogWrite на 255) должна быть скорость 100 об/мин = 600 град/сек, то при половине напряжения (analogWrite на 128) должна быть скорость 50 об/мин = 300 град/сек и т.д. На графиках она будет отмечена синим.
Итак, посмотрим на угловую скорость выходного вала мотора, вычисленную в обработчике внешних прерываний (на каждом тике энкодера):
Синяя линия — предполагаемая скорость, красная — вычисленная. Она отстает, но тоже выглядит линейно. Только на очень малых значениях (при переходе через нуль) присутствуют «ступеньки» — паузы между тиками слишком большие и не помещаются в переменную типа word, поэтому скорость вычисляется некорректно.
Теперь посмотрим на скорость, вычисленную в обработчике прерываний таймера:
На графике явно присутствует «иголки» — резкие переходы между соседними точками, это вызвано низкой точностью измерений. Кроме того, можно заметить «отставание» красного графика от синего — «вершины» сигналов не на одной вертикали, у вычисленного она смещена правее. То есть, мотор уже давно отрабатывает другую скорость, а таймер еще не досчитал, и обновленная скорость еще не вычислялась.
Объединим оба метода, посмотрим на графики:
Радует то, что значения скоростей совпадают (оба метода вычисляют адекватные скорости). Даже если давать нагрузку на мотор и таким образом притормаживать его, оба графика будут синхронно увеличиваться и уменьшаться:
Красным обозначен метод определения скорости по таймеру, обратите внимание, что при перепадах красная линия всегда правее зеленой, то есть этот метод «отстает» от метода определения скорости на каждом тике энкодера.
Теперь я попробую изменять период таймера, посмотрим, что из этого выйдет:
Как видим, наиболее гладкие графики получаются с таймером, настроенным на 20-50 мс. Если период уменьшать, то слишком сильно снижается точность, если период увеличивать, то вычисление скорости происходит слишком редко и ее значение «отстает» от реального.
Давайте сделаем выводы…
… о том, какой способ измерения лучше? Очевидно, что гораздо точнее измерять скорость на каждом тике энкодера. Этот способ имеет единственное ограничение — во время полной остановки мотора прерывания не происходят и скорость не вычисляется, в переменной хранится последнее вычисленное значение. Ограничение по минимальной скорости можно обойти изменением типа переменной с word на unsigned long. Я думаю, если в прерывании успевало происходить вычисление угловой скорости с операциями деления, другими «тяжелыми» переменными, то и сохранение времени в четыре байта успеет пройти.
Способ с прерываниями по таймерам может измерять даже нулевую скорость у остановившегося мотора. Он лучше подойдет для энкодеров, присылающих «тики» гораздо чаще. Можно увеличить точность этого способа в 4 раза, используя считывание не тиков, а состояний энкодера. Реализовать подобное для нескольких моторов возможно только на платах Arduino продвинутого уровня, с множеством внешних прерываний и таймеров. С другой стороны, эти платы часто построены на нестандартных микроконтроллерах, имеющих другую реализацию таймеров, не поддерживаемую в широко распространенных библиотеках. Поэтому придется вчитываться в даташиты и настраивать таймера с помощью регистров. Лично я считаю, что результат не стоит затраченных на это усилий.
Еще один способ улучшить точность измерений в таймере — запоминать сразу несколько предыдущих показаний энкодера, например 10, и сравнивать текущее значение не с самым последним из них, а с более ранними. Таким образом разница увеличится, увеличится точность и можно будет период таймера немного уменьшить. Думаю, это тема для новых экспериментов и отдельной статьи. Или любой из вас может попробовать этот способ и описать результаты, поделиться опытом в комментариях.
Для выбранного мотор-редуктора Pololu 25mm наиболее подходящим и простым способом является вычисление скорости на каждом тике энкодера.
