Обработка сигналов энкодеров в Arduino

Методист по олимпиадной робототехнике Университета Иннополис Алексей Овсянников рассказывает, как обработывать сигналы энкодеров двигателей робота в Arduino.

Продолжаю публиковать материалы для слушателей курсов повышения квалификации по подготовке к Innopolis Open in Robotics. В прошлый раз я рассказал про то, как были подготовлены сцены Манипуляционных ИРС в симуляторе CoppeliaSim. Сегодня я хотел бы обратить внимание на механическую часть робота.

Множество команд используют для создания робота широко распространенный набор Lego Mindstorms EV3 или аналогичные (VEX и подобные). Их особенность в использовании датчиков и приводов всего нескольких видов, строго определенных производителем. Контроллеры получают информацию о повороте вала мотора с точностью до градуса, могут определить текущую скорость вращения мотора или синхронизировать их (замедлить любой мотор при замедление другого и т.д.) буквально одной командой.

Олимпиада Innopolis Open in Robotics не ограничивает участников в выборе оборудования (кроме очевидного запрета на использование готовых, фабричной сборки, роботов в некоторых номинациях). Вопрос лишь в том, как использовать всю широту ассортимента китайских магазинов.

Алгоритмы синхронизации моторов хорошо описаны в пособии «Управление моторами тележки с контроллером Трик на JavaScript«. Олег Киселев, автор пособия, описывает алгоритмы, которые можно перенести на любую другую платформу. Но и в этих примерах обращения к датчикам оборотов — энкодерам — происходит через готовые команды el.reset(), er.read() и подобные. Если же использовать в качестве контроллера робота Arduino, ESP- или STM-платы, то подобные команды придется реализовывать самостоятельно.

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

Рассмотрим принцип действия энкодера. Чаще всего в простых дешевых системах используются квадратурные инкрементные энкодеры. Эти страшные слова означают, что они выдают прямоугольные импульсы (резко, а не плавно, возникающий и пропадающий сигнал) и, посчитав эти импульсы, можно понять, насколько провернулся мотор. Какого-то определенного начального положения, начала отсчета, у энкодера нет. Подсчет потребуется реализовывать в программе контроллера. Проще всего объяснить работу оптического энкодера, который содержит диск-крыльчатку с прозрачными и непрозрачными областями (реже — с отражающими и не отражающими областями), источник и приемники оптического излучения.

Оптический сигнал проходит через «окна» в диске и попадает на фотоприемники, которые выдают сигнал. Если свет не проходит, то сигнала нет. Чем быстрее вращается диск, тем короче становятся импульсы. Таким образом измеряется скорость вращения. А как определить направление? Для этого ставят два приемника, таким образом, чтобы один из них открывался в тот момент, когда второй «видит» как раз середину окна (показывает середину импульса).

Обратите внимание, что при вращении диска по часовой стрелке сперва «открывается» приемник А — импульс на нем появляется раньше, чем на приемнике В. При вращении против часовой стрелки сигнал появляется сперва на приемнике B, потом на А. Отслеживание и сравнение сигналов позволяет понять направление вращения диска и вала, к которому он закреплен.

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

Еще раз присмотримся к сигналам, приходящим с приемников A и В: за полный период (цикл, который повторяется при отслеживании одного «окна» на диске) есть четыре состояния выходов. Можно отслеживать их все и увеличить точность измерений или отслеживать только один выход (А или В), а по второму определять направление. Какого только колхоза в программах, отслеживающих энкодеры, я ни встречал. Кто-то пытается ловить код единицами и нулями, которые обозначены на рисунке, кто-то делает пятиэтажные опросы. Но об этом поговорим позже, возможно даже, не в этой статье.

Возьмем для примера два распространенных мотора: Pololu 25mm metal gearmotor и TETRIX MAX DC Motor. Оба они являются мотор-редукторами, что означает совмещение электромотора и редуктора в одном устройстве. Электромотор вращается очень быстро, но слабо, а редуктор увеличивает усилие, снижая скорость вращения. В итоге на выходном валу мотор-редуктора мы получаем меньшую частоту вращения, но большее усилие. Pololu позволяет выбрать один из нескольких вариантов с разными редукторами. То есть, сам мотор может оставаться тем же самым, а меняя редуктор, мы можем получить разные характеристики на выходном валу.

Пока что лучше перейдем к энкодеру TETRIX DC Motor:

Он устанавливается на выходном валу мотор-редуктора. На вал крепится диск с рисками (отражающими и не отражающими областями). Таких рисок аж 1440 на диске, то есть за один оборот вала энкодер может насчитать 1440 импульсов на одном выходе или в четыре раза больше состояний. Точность измерений 0,25 градуса (4 импульса на 1 градус) или 0,0625 градуса (16 состояний на 1 градус). Внушительно!

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

Теперь посмотрим на мотор-редуктор Pololu с энкодером:

На рисунке я выдели цветами:

Красный — выходной вал мотор-редуктора

Зеленый — редуктор

Оранжевый — электромотор

Голубой — энкодер

В случае мотора Pololu и распространенных моделей 25GA-370 энкодер вешается с обратной стороны электромотора на выходящий вал. Энкодер считает обороты не самого мотор-редуктора, а только мотора. Зная передаточное число редуктора можно рассчитать угол поворота выходного вала относительно показаний энкодера.

Подобный подход имеет важный минус, кроме очевидной необходимости проводить дополнительные расчеты, а именно — наличие погрешности измерений из-за зазоров и упругих деформациях в редукторе. Выходной вал может повернуться на долю градуса при неподвижном вале мотора. Но в большинстве случае подобные погрешности не превышают одного градуса, а проявляются при резких сменах направления вращения. А простота изготовления диска всего с 10-20 оптическими окнами или магнитными областями значительно удешевляет конструкцию. Моторы серии 25GA-370 стоят по 900 рублей вместе с энкодерами, фирменный мотор Pololu с энкодером обойдется примерно в 3000 рублей.

Посчитаем, какая точность измерений у энкодеров, расположенных на валу мотора, а не выходном валу мотор-редуктора. Для этого обратимся к сравнительной таблице моторов Pololu. Ограничимся какой-нибудь одной серией, например, 12В medium power:

Мотор-редуктор с передаточным отношением 75:1 вращает выходным валом со скоростью 100 оборотов в минуту, а при передаточном отношении 172:1 выходной вал делает 43 оборота за минуту. Просматривая страницы каждого мотора можно заметить, что реально передаточные отношения немного отличаются (74,83 и 171,79 для указанных ранее).

Посчитаем, сколько импульсов энкодер выдает за один оборот выходного вала. За один оборот магнитного диска (то есть, вала мотора) энкодер Pololu выдает по 12 импульсов на каждом выходе, энкодер моторов серии 25GA-370 по 11 импульсов. За один оборот выходного вала вал мотора делает количество оборотов, равное передаточному отношению редуктора. Для моторов Pololu выбранной серии — от 1 до 227. Для моторов серии 25GA-370 — от 4,4 до 500. Получим следующие значения (в таблицу подставлены точные значения передаточных отношений мотор-редукторов Pololu):

Чем меньше скорость вращения выходного вала, тем точнее измерения энкодера. Для наиболее ходовых и применимых в мобильных платформах моделях на 70-170 об/мин (выделены зеленым) даже простой подсчет импульсов на одном выходе энкодера дает точность менее 1 градуса. Меньше, чем 1440 импульсов и 5760 состояний энкодера TETRIX, но сопоставимо с моторами Lego.

Итоговые формулу, связывающие сигналы энкодера с градусами поворота выходного вала будут следующие:

LEGO® Education WeDo (9580)-11220 руб. Mindstorms EV3-34650 руб.

где:

n — «тики», сигналы энкодера;

i — передаточное отношение редуктора;

IPR — (impulse per rotation) кол-во импульсов энкодера на 1 оборот диска (может быть как 12 импульсов, так и 48 состояний, в зависимости от того, что отслеживается в программе).

Теперь посчитаем, как часто приходят сигналы с датчика. Умножив частоту вращения выходного вала на передаточное отношение можно получить скорость вращения электромотора. Для всех моторов Pololu она составляет примерно 7500-7800 оборотов в минуту. Фирма Pololu указывает частоту вращения холостого хода (мотора без нагрузки), но сам редуктор может выступать некоторой нагрузкой, поэтому частота вращения и отличается. Так как самая быстрая частота вращения как раз на холостом ходу, а под нагрузкой мотор будет замедляться, то как максимально возможную принимаем именно ее.

Для популярной серии моторов 25GA, взяв за основу таблицу с сайта DVRobot.ru, можно вычислить скорость вращения мотора примерно как 5950-6000 оборотов в минуту.

Округлим скорости вращения в большую сторону и возьмем 7800 об/мин для Pololu 12V medium power и 6000 об/мин для 25GA-370. Обе серии широко распространены и подходят для подключения через драйвера на основе L298P или L298N.

Итак, диск энкодера делает по 7800 или 6000 оборотов за минуту (обозначим частоту вращения символом ?). Энкодер двигателей Pololu выдает по 12 импульсов (IPR=12) на каждом выходе за один оборот. Итого:

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

Энкодер моторов 25GA-370 выдает по 11 импульсов на каждый выход за один оборот. Для него получаем

Или 264 000 состояний за минуту.

Делим эти огромные числа на 60 и получаем кол-во импульсов и состояний за секунду. Вычисляем период одного импульса и состояния (время, за которое они сменяются).

У моторов Pololu на холостом ходу каждый новый импульс приходит раз в 641 микросекунду, у 25GA-370 раз в 909 микросекунд. Необходимо, чтобы наш управляющий контроллер успевал фиксировать эти импульсы. Стандартная плата Arduino UNO работает на частоте 16 МГц, то есть, делает 16 млн тактов в секунду. Один такт занимает 0,0625 мкс. Контроллеры на базе STM или ESP работают на больших частотах, их такты гораздо меньше. Желтым в таблице обозначены количества тактов, которые успевает сделать Arduino UNO за время смены одного состояния или прихода одного импульса. Может показаться, что даже 2564 тактов на считывание состояния энкодера Pololu будет более чем достаточно, но вот тут и начинает проявляться «колхоз» в программной обработке показаний датчиков. К сожалению, та же плата Arduino UNO не умеет выполнять действия в параллельных потоках, только в основном цикле. Достаточно нагрузить его сложными вычислениями дробных чисел или считыванием аналоговых датчиков (одни из самых долгих операций; паузы командой delay и работу с интерфейсом UART-Serial оставляем за скобками, они вне конкуренции) и каждая его итерации начнет занимать по несколько тысяч тактов. Считывание показаний в этом датчике неизбежно приведет к пропуску тактов. Выход из этой сложной ситуации кроется в использовании прерываний. Это специальные подпрограммы, которые выполняются при наступлении определенных событий. Например, их можно настроить на появление сигнала на пине. То есть, когда приходит импульс от энкодера, Arduino прерывает основной цикл программы, выполняет небольшой кусочек кода и возвращается в основной цикл в то же самое место, где прервалось выполнение. С точки зрения основной программы, ничего и не произошло. Важно понимать, что вызываемая по прерыванию подпрограмма должна быть как можно меньше и выполняться как можно быстрее, чтобы надолго не прерывать основную программу. Иначе можно получить ситуацию накопления прерываний — пока обрабатывается одно прерывание, происходит следующее событие и основная программа просто не успевает выполняться.

У платы Arduino UNO внешние прерывания можно повесить только на два пина: 2 и 3. Так как управлять хочется двумя моторами, получаем всего по одному прерыванию на мотор. Следовательно, будем отслеживать появление сигналов на одном выходе энкодера, а второй выход покажет направление вращения. Работа с прерываниями сводится к следующим шагам:

1. Необходимо написать подпрограммы, которые будут вызываться. Они обязательно должны быть void (не возвращать никаких значений и без параметров):

Попав в подпрограмму я узнаю, какой сигнал на втором выходе энкодера и, в зависимости от результата, увеличиваю или уменьшаю счетчик encA / encB. Обратите внимание, что эти счетчики должны быть самого вместительного типа, у меня это long, так как обычный int заполнится за минуту-две непрерывного движения.

Присоединить прерывание, то есть включить его. Делается это командой attachInterrupt() со следующими параметрами:

attachInterrupt(digitalPinToInterrupt(pin), ISR, mode)

где

pin — пин, к которому привязывается прерывание (для Arduino UNO это пины 2 и 3, для других плат смотрите описание команды),

ISR — имя вызываемой подпрограммы,

mode — режим срабатывания, бывает LOW (срабатывает пока 0 на пине), CHANGE (срабатывает при любом изменении цифрового сигнала на пине), RISING (срабатывает при изменении сигнала с 0 на 1 на пине), FALLING (срабатывает при изменении сигнала с 1 на 0 на пине). В нашем случае логичнее использовать RISING.

Итоговая программа у меня получилась такой:

Неважно, что команды Serial.print() в основном цикле выполняются очень медленно, при поступлении сигналов от энкодеров они будут прерываться. Пока что программа просто выводит текущие показания энкодеров по UART’у, но используя формулы из статьи можно вычислять и угол поворота выходного вала. Попробуйте сами написать эти вычисления с учетом параметров своего мотор-редуктора и датчика.

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

Листинги программ:
#define INT_A_PIN 2
#define INT_B_PIN 3
#define SIG_A_PIN 4
#define SIG_B_PIN 5
long encA = 0;
long encB = 0;
void setup() {
pinMode(INT_A_PIN, INPUT);
pinMode(INT_B_PIN, INPUT);
pinMode(SIG_A_PIN, INPUT);
pinMode(SIG_B_PIN, INPUT);
Serial.begin(9600);
attachInterrupt(digitalPinToInterrupt(INT_A_PIN), EncA, RISING);
attachInterrupt(digitalPinToInterrupt(INT_B_PIN), EncB, RISING);
}
void EncA()
{
if ( digitalRead(SIG_A_PIN) )
{
encA++;
} else {
encA--;
}
}
void EncB()
{
if ( digitalRead(SIG_A_PIN) )
{
encB--;
} else {
encB++;
}
}
void loop() {
Serial.print(encA);
Serial.print(" ");
Serial.println(encB);
}

Источник: edurobots.ru

Комментарии: