В предыдущей статье «Проектирование оборудования и схем ChefBot» мы обсудили выбор аппаратных компонентов, необходимых для сборки нашего робота. Основные компоненты – это датчики и привод робота. Приводы обеспечивают мобильность робота, а датчики предоставляют информацию об окружающей среде.
В этой статье мы уделим особое внимание различным типам приводов и датчиков, которые мы собираемся использовать в этом роботе, и их сопряжению с Tiva C LaunchPad. Tiva C LaunchPad – это 32-битный микроконтроллер ARM от Texas Instruments, работающий на частоте 80 МГц.
Начнем статью с обсуждения приводов. Рассматриваемый нами привод – это двигатель постоянного тока, совмещенный с редуктором и энкодером. Редуктор предназначен для уменьшения частоты вращения ведущего вала и увеличения крутящего момента. Этот вид двигателей очень экономичен и удовлетворяет нашим требованиям к конструкции робота.
В первой части данной статьи мы рассмотрим конструкцию системы привода робота. Система привода робота состоит из двух двигателей – редукторов постоянного тока с энкодерами и драйвером двигателя. Драйвер двигателя контролирует Tiva C LaunchPad. Мы рассмотрим сопряжение драйвера двигателя и квадратурного энкодера с LaunchPad Tiva C. Далее мы рассмотрим некоторые из новейших приводов, которые могут заменить существующий двигатель постоянного тока с редуктором и энкодером. Если роботу будет необходимо перевозить более тяжелую полезную нагрузку или потребуется повышенная точность, следует использовать более новый привод.
И в конце статьи нами будут рассмотрены различные датчики, обычно используемые в мобильных роботах.
Итак, в этой статье мы рассмотрим:
- подключение двигателя постоянного тока с редуктором к Tiva C к LaunchPad;
- взаимодействие квадратурного энкодера с Tiva C LaunchPad;
- объяснение кода взаимодействия;
- взаимодействие приводов Dynamixel;
- сопряжение ультразвуковых датчиков и ИК-датчиков сближения;
- взаимодействие инерциальных измерительных блоков (IMUs).
Технические условия
Вам понадобятся компоненты оборудования робота и установленный в Ubunru 16.04 LTS компонент под названием Energia IDE. Energia – это инструмент редактирования кода с открытым исходным кодом, разрабатываемый сообществом для LaunchPad.
Согласование редукторного двигателя постоянного тока с Tiva C LaunchPad
В предыдущей статье «Проектирование оборудования и схем ChefBot» нами был выбран двигатель постоянного тока со встроенным редуктором и встроенным энкодером от Pololu (возможно использование альтернативного варианта) и мы подключали к микроконтроллеру Tiva C LaunchPad. Нам для взаимодействия двигателей с LaunchPad понадобятся следующие компоненты:
- два двигателя с металлическим редуктором pololu 37Dx57L mm с 64 импульсами, вырабатываемыми энкодером за 1 оборот вала;
- колеса Pololu размером 90×10 мм и ступица для соединения колеса и вала редуктора;
- сдвоенный драйвер двигателя VNH2SP30, MD03A с сайта Pololu (или альтернатива);
- герметичный свинцово-кислотный или литий-ионный аккумулятор напряжением 12 В;
- преобразователь логических уровней от 3,3 до 5 В (https://www.sparkfun.com/products/12009);
- Tiva C LaunchPad и совместимый интерфейс.
На следующем рисунке показана схема взаимодействия двух двигателей с помощью H-моста.
Для взаимодействия двигателей с LaunchPad нам необходимо соединить микроконтроллер с двумя двигателями ходовой части робота. Рабочее напряжение драйвера двигателя равно 5 В, а контроллер LaunchPad вырабатывает напряжение 3,3 В. Потому для согласования микроконтроллера и двигателей мы должны подключить двигатели к микроконтроллеру с помощью преобразователей, поэтому мы должны подключаться к уровню переключения, как показано на следующей схеме.
Оба редукторных двигателя постоянного тока подключены к драйверу двигателя через контакты OUT1A, OUT1B и OUT2A, OUT2B. Питающее напряжение двигателей подается через контакты VIN (+) и GND (–). Эти двигатели постоянного тока работают от напряжения 12 В, поэтому мы и подаем входное напряжение, равное 12 В. Драйвер двигателя обеспечивает входное напряжение в диапазоне от 5,5 до 16 В.
Первый контакт 1DIAG/EN в большинстве случаев остается неподключенным (см. верхнюю схему подключения двух двигателей к Polou MD03A Dual VNH2SP30). Эти контакты предназначены для управления (включения или выключения) H-моста и контроля его состояния. Сигналы управления направлением вращения двигателя поступают на контакты 1INA и 1INB. Сигнал на включение или отключение двигателя поступает на контакт 1PWM. Скорость вращения двигателя контролируется его кратковременным включением и отключением. Контрольные данные с датчика тока поступают на контакт CS. При выходном токе, равном 1 А, на контакте CS появится сигнал с напряжением 0,13 В. Контакты VIN (напряжение питания 12 В и GND (земля)) не используются. Питание драйвера двигателя снимается с контактов +5 В (IN) и GND. Управление драйверами каждого двигателя поступает на отдельные контакты. Так, сигналы для управления направлением вращения первого двигателя поступают на контакты 1INA и 1INB, а сигналы для управления направлением вращения второго двигателя подаются на контакты 2INA и 2INB.
В следующей таблице показана таблица истинности комбинаций входных и выходных данных.
Управляя двигателями с помощью этих сигналов, мы можем перемещать робота в любую точку окружающего пространства. А управляя скважностью импульсов (широтно-импульсная модуляция), мы управляем скоростью вращения двигателя. Это основная логика управления двигателем постоянного тока с помощью Н-моста.
Для взаимодействия драйвера двигателей с LaunchPad необходим преобразователь логических уровней 3,3–5 В. Это объясняется тем, что напряжение на контактах LaunchPad равно 3,3 В, а напряжение на контактах драйвера двигателя равно 5 В. Поэтому нам необходимо с помощью логического преобразователя преобразовать сигнал с 3,3 до 5 В и наоборот.
В дифференциальном приводе используется два двигателя. Далее мы обсудим работу дифференциального привода.
Дифференциальный привод колесного робота
Как мы уже многократно упоминали, разрабатываемый нами робот – робот с дифференциальным колесным приводом. В дифференциальном колесном роботе механизм управления основан на независимом вращении двух отдельно управляемых колес, расположенных по обе стороны корпуса робота. Он может менять направление движения, используя разность в скорости вращения колес. Поэтому ему не требуется рулевое управление.
Для поддержки корпуса робота используются поддерживающие колеса.
На следующем рисунке показано типичное представление дифференциального привода.
Если валы двигателей будут вращаться в одном направлении, робот будет двигаться вперед или назад. Если скорость вращения одного двигателя будет превышать скорость вращения второго двигателя, робот будет совершать поворот в сторону двигателя с меньшей скоростью вращения. Если правый двигатель остановлен, а левый работает, то робот будет поворачивать вправо, для левого поворота необходимо остановить левый двигатель, а вал редуктора правого двигателя должен вращаться по часовой стрелке. На следующем рисунке показана установка двигателей нашего робота. Два двигателя смонтированы на противоположных сторонах опорной пластины. К ведущим валам редукторов каждого двигателя крепятся ведущие колеса. Для поддержки робота используются опорные колеса.
Далее, согласно таблице истинности, используя LaunchPad, мы можем запрограммировать драйвер двигателя. Программирование осуществляется с помощью IDE под названием Energia (http://energia.nu/). LaunchPad программируется с помощью языка Wiring (http://wiring.org.co/).
Установка Energia IDE
Последнюю версию Energia можно скачать по следующей ссылке: http://energia.nu/download/. Далее мы рассмотрим порядок установки на 64-разрядную Ubuntu 16.04.LTS. Используемая нами версия Energia – 0101E0018.
1. Загрузите Energia для Linux 64-разрядной версии по ссылке выше.
2. Извлеките Energia из архива в домашнюю папку пользователя.
3. Установите плату микроконтроллера Tiva C.
4. Загрузите файл 71-ti-permissions.rules по следующей ссылке:
https://gist.github.com/alairjunior/a172fc07e102cb84976e3587108e1fd1.
5. Файл правил даст разрешение пользователю на чтение и запись в LaunchPad. Вы должны сохранить файл под именем 71-ti-permissions.rules и, чтобы скопировать файлы правил в системную папку для получения разрешения, выполнить следующую команду из текущего пути:
$ sudo mv 71-ti-permissions.rules /etc/udev/rules.d/
6. После копирования файла выполните команду для активации правила:
$ sudo service udev restart
7. Теперь вы можете подключить плату микроконтроллера Tiva C LaunchPad к компьютеру и в терминале Linux выполнить команду dmesg. Эта команда позволит просмотреть журнал ядра Linux. Если плата микроконтроллера подключена, то устройство последовательного порта (COM) в конце сообщения отобразит строку (см. следующий рисунок).
8. Если в терминале появилось сообщение о том, что микроконтроллер к COM-порту подключен, используя следующую команду, запустите Energia:
$./energia
Запуск Energia IDE показан на следующем рисунке.
Теперь нам нужно выбрать плату tm4c123 в IDE для компиляции специального кода для этой платы. Чтобы выбрать плату tm4c123, следует установить ее пакеты.
Для установки пакетов выберите опцию Tools → Boards → Boards Manager.
9. Выберите вкладку Tools → Boards → Launchpad (Tiva C) w/tm4c123 (80MHz) и, как показано на следующем рисунке, микроконтроллер.
10. Перейдите в меню Tools → Serial Port → /dev/ttyACM0 и выберите последовательный порт.
11. Скомпилируйте и загрузите код с помощью кнопки Upload. Кнопка Upload выполнит оба процесса. Следующий снимок экрана иллюстрирует успешную загрузку.
Код взаимодействия с двигателями
Следующий код используется для тестирования двух двигателей в дифференциальном приводе. Этот код перемещает робот в течение 5 с вперед и в течение 5 с назад. Затем робот в течение 5 с перемещается влево, далее в течение 5 с вправо. После каждого движения робот останавливается на 1 с.
В начале кода мы определяем контакты для INA, INB и PWM двух двигателей следующим образом:
///Пины левого мотора
#define INA_1 12
#define INB_1 13
#define PWM_1 PC_6
///Пины правого мотора
#define INA_2 5
#define INB_2 6
#define PWM_2 PC_5
Следующий код содержит пять функций для перемещения робота вперед, назад, влево и вправо. Пятая функция – остановка робота. Мы будем использовать функцию записи цифрового значения PIN-кода digitalWrite(). Первый аргумент digitalWrite() является номером пина, а второй аргумент – это значение, которое должно быть записано для пина. Значение может быть ВЫСОКИМ или НИЗКИМ. Мы будем использовать функцию analogWrite() для записи значения пину PWM. Первым аргументом этой функции является число пинов, а вторым – значение ШИМ. Диапазон этого значения – от 0 до 255. При высокой частоте ШИМ драйвер двигателя включает и отключает двигатель с высокой частотой. Поэтому двигатель будет вращаться с большой скоростью. На низкой частоте ШИМ частота переключения драйвера двигателя будет низкой, и скорость вращения вала тоже будет низкой. Текущее значение скорости вращения будет высоким.
voidmove_forward()
{
//Установим вращение левого мотора по часовой стрелке, а вращение правого мотора против
// часовой стрелки
//Левый мотор
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,LOW);
analogWrite(PWM_1,255);
//Правый мотор
digitalWrite(INA_2,LOW);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,255);
}
///////////////////////////////////////////////////////
voidmove_left()
{
//Левый мотор
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,HIGH);
analogWrite(PWM_1,0);
//Правый мотор
digitalWrite(INA_2,LOW);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,255);
}
//////////////////////////////////////////////////////
voidmove_right()
{
//Левый мотор
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,LOW);
analogWrite(PWM_1,255);
//Правый мотор
Согласование редукторного двигателя постоянного тока с Tiva C LaunchPad 145
digitalWrite(INA_2,HIGH);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,0);
}
////////////////////////////////////////////////////////
void stop()
{
//Левый мотор
digitalWrite(INA_1,HIGH);
digitalWrite(INB_1,HIGH);
analogWrite(PWM_1,0);
//Правый мотор
digitalWrite(INA_2,HIGH);
digitalWrite(INB_2,HIGH);
analogWrite(PWM_2,0);
}
/////////////////////////////////////////////////
voidmove_backward()
{
//Левый мотор
digitalWrite(INA_1,LOW);
digitalWrite(INB_1,HIGH);
analogWrite(PWM_1,255);
//Правый мотор
digitalWrite(INA_2,HIGH);
digitalWrite(INB_2,LOW);
analogWrite(PWM_2,255);
}
Контакты INA и INB для обоих двигателей – это входные контакты. Поэтому на этих контактах может присутствовать как высокое, так и низкое значение (логическая единица или логический ноль). Функция pinMode() используется для установки режима ввода/вывода. Первый аргумент pinMode() – номер контакта, второй аргумент – это режим. Мы можем контакт назначить как входной, так и выходной. Чтобы назначить контакт как выходной, запишите OUTPUT (выходной аргумент) в качестве второго аргумента; чтобы назначить контакт как входной, запишите INPUT (входной аргумент) в качестве второго аргумента. Это показано в следующем коде. Нет необходимости назначать контакт PWM в качестве выходного, потому что analogWrite() назначает без установки режима вывода pinMode():
void setup()
{
//Установим пины левого мотора на приём сигнала — OUTPUT
pinMode(INA_1,OUTPUT);
pinMode(INB_1,OUTPUT);
pinMode(PWM_1,OUTPUT);
//Установим пины правого мотора на приём сигнала – OUTPUT
pinMode(INA_2,OUTPUT);
pinMode(INB_2,OUTPUT);
pinMode(PWM_2,OUTPUT);
}
Следующий фрагмент является основным циклом кода. Он в течение 5 с вызывает каждую функцию: move_forward(), move_backward(), move_left() и move_right(). После вызова каждой функции робот останавливается на 1 с.
void loop()
{
//Движение вперед 5 с
move_forward();
delay(5000);
//Остановка 1 с
stop();
delay(1000);
//Движение назад 5 с
move_backward();
delay(5000);
//Остановка 1 с
stop();
delay(1000);
//Поворот налево 5 с
move_left();
delay(5000);
//Остановка 1 с
stop();
delay(1000);
//Поворот направо 5 с
move_right();
delay(5000);
//Остановка 1 с
stop();
delay(1000);
}
Интерфейс квадратурного энкодера с Tiva C Launchpad
Колесный энкодер – это датчик, прикрепленный к двигателю и определяющий количество оборотов колеса. Если мы знаем количество оборотов, то можем вычислить смещение, скорость, ускорение и угол поворота колеса.
Как уже упоминалось ранее, для данного робота мы выбрали мотор со встроенным энкодером. Этот энкодер квадратурного типа определяет направление и скорость вращения вала двигателя. В энкодерах могут быть использованы различные типы датчиков, такие как оптические датчики или датчики Холла. Данный энкодер для определения характеристик вращения использует эффект Холла. Квадратурный энкодер имеет два канала, а именно канал A и канал B. Каждый канал генерирует цифровые сигналы со сдвигом на 90°. На следующем рисунке показана осциллограмма типичного квадратурного энкодера.
Если вал двигателя вращается по часовой стрелке, то фронт импульса канала А будет опережать фронт импульса канала B. Если вал двигателя вращается против часовой стрелки, то фронт импульса канала B будет опережать фронт импульса канала А. Эти значения используются для определения направления вращения вала двигателя. Далее мы обсудим, как перевести показания энкодера в значения скорости и смещения.
Обработка данных энкодера
Данные датчика представляют собой двухканальный импульсный сигнал со сдвигом фронтов импульсов на 90° по фазе. Используя эти данные, мы можем определить направление вращения и количество оборотов вала двигателя. С помощью этих данных мы определим скорость вращения вала двигателя и перемещение робота.
Работу энкодера характеризуют следующие значения: количество импульсов на оборот (PPR), или количество линий на оборот (LPR), и количество счета на оборот (CPR). PPR – это количество электрических импульсов на один оборот вала двигателя. Некоторые производители вместо PPR используют название CPR, так как каждый импульс состоит из двух вертикальных линий: переднего фронта (нарастание амплитуды импульса), когда сигнал скачкообразно растет от 0 до 1, и спада, когда амплитуда сигнала уменьшается от 1 до 0. Энкодер имеет два канала, каждый из которых генерирует импульсный сигнал. Фронты импульсов канала А и канала В сдвинуты по фазе по отношению друг к другу на 90°. Общее число линий (фронтов и спадов) будет в четыре раза больше количества импульсов. Большинство квадратурных приемников использует так называемое 4X-декодирование для подсчета всех пиков от A и B-каналов энкодера, уступая 4Х-разрешению по сравнению с грубыми значениями PPR.
В нашем двигателе pololu указывается значение CPR, равное 64 на валу двигателя. Это соответствует 8400 CPR на выходном валу редуктора. По сути дела, мы получаем 8400 CPR на выходном валу редуктора, на один оборот вала двигателя. На следующем рисунке показано, как вычисляется количество импульсов энкодера.
В этой спецификации энкодера задается количество импульсов на оборот; данное значение рассчитывается по количеству пиков, проходящих через канал энкодера. Один импульс канала энкодера соответствует четырем пикам. Таким образом, чтобы получить значение 8400 в нашем двигателе, необходимо, чтобы PPR был 8400/4 = 2100. Из предыдущего рисунка мы сможем рассчитать количество в одном обороте, но мы также должны определить и направление вращения. Количество импульсов не зависит от направления вращения вала двигателя и одинаково при вращении как по часовой, так и против часовой стрелки. Но для декодирования сигнала важно знать направление движения. На следующем рисунке показано, как декодировать импульсы энкодера.
Если мы понаблюдаем за структурой кодированного сигнала, то можем понять, что он соответствует 2-битному Gray-кодированию. Gray-код кодирует числа так, что два соседних значения различаются только в одном разряде. Gray-коды широко используются в роторных энкодерах для эффективного кодирования.
Подробнее о Gray-коде читайте здесь: http://en.wikipedia.org/wiki/Gray_code.
Можно предсказать направление вращения двигателя по переходным состояниям. Таблица перехода приведена ниже.
Будет удобнее представить переходы на схеме переходного состояния:
Получив Gray-код, мы можем обработать поступающие импульсы с помощью микроконтроллера. Контакты каналов двигателя необходимо подключить к контактам прерывания микроконтроллера. Таким образом, когда в канале появятся реберные переходы, будет генерироваться прерывание. При появлении на контакте сигнала прерывания микроконтроллер выполнит функцию прерываний. В соответствии с текущим состоянием контактов и их предыдущих значений мы определяем направление вращения вала двигателя и решаем, нужно ли увеличивать или уменьшать количество прерываний (импульсов). Это и есть основная логика обработки энкодера.
Получив значение счетчика, можно вычислить угол поворота (в градусах) с помощью выражения: Угол = (Значение счетчика / CPR)×360. Если заменить CPR на 8400, уравнение принимает такой вид: Angle = 0,04285×Значение счетчика, то есть для поворота на один градус должно быть получено 24 отсчета, или шесть закодированных импульсов канала.
На следующем рисунке показана схема взаимодействия энкодера одного из двигателей с Tiva C LaunchPad.
Максимальный уровень выходного импульса от энкодера – от 0 до 5 В. В этом случае мы можем напрямую воздействовать на энкодер с LaunchPad, так как микроконтроллер работает с сигналами до 5 В. Если энкодер не может развить сигнал до уровня 5 В, следует применить преобразователь уровня от 3,3 до 5 В, как это мы делали для драйвера двигателя раньше.
В следующей части статьи мы напишем код для Energia, который позволит протестировать сигнал квадратурного энкодера. Нам необходимо убедиться, получим ли мы правильное значение счетчика от энкодера.
Код согласования квадратурного энкодера
Этот код выведет данные счетчика левого и правого энкодеров двигателя через последовательный порт. Два энкодера находятся в схеме декодирования 2X, поэтому мы получим 4200 CPR. В первом разделе кода мы определяем контакты для двухканальных выходов двух энкодеров и даем описание переменной count для двух энкодеров. Переменная энкодера использует ключевое слово volatile перед типом переменных данных.
Основное использование volatile состоит в том, что переменная с volatile keyword будет сохранена в оперативной памяти, а обычные переменные – в регистре процессора. Значения кодировщика меняются очень быстро, поэтому использование обычной переменной будет неточным. Чтобы получить точность, будем использовать переменные volatile для энкодера, как показано ниже:
//Определение пинов энкодера
// Левый энкодер
#define Left_Encoder_PinA 31
#define Left_Encoder_PinB 32
volatile long Left_Encoder_Ticks = 0;
// Переменная для чтения текущего состояния левого датчика энкодера
volatile bool LeftEncoderBSet;
//Правый энкодер
#define Right_Encoder_PinA 33
#define Right_Encoder_PinB 34
volatile long Right_Encoder_Ticks = 0;
// Переменная для чтения текущего состояния правого датчика энкодера
volatile bool RightEncoderBSet;
Следующий фрагмент кода является определением функции setup(). В языке wiring setup() – это встроенная функция, используемая для инициализации и выполнения действий на один такт для переменных и функций. Внутри setup() мы инициализируем последовательную передачу данных со скоростью передачи данных 115 200 и вызов определяемой пользователем функции SetupEncoders() для инициализации контактов энкодера. В основном передача последовательных данных используется для проверки значения счетчика энкодера через serial terminal.
void setup()
{
// Последовательный порт со скоростью передачи 115 200 бод
Serial.begin(115200);
SetupEncoders();
}
Определение SetupEncoders() приводится в следующем коде. Для получения импульса энкодера нам понадобятся два контакта LaunchPad в качестве входных. Настроим входные контакты энкодера на LaunchPad с помощью нагрузочного резистора. Функция attachInterrupt() настроит один из выводов энкодера в качестве прерывания. Функция attachInterrupt() имеет три аргумента. Первый аргумент – это значение pin, второй – процедура обслуживания прерывания (ISR), и третьим аргументом является условие прерывания, то есть условие, при котором запустится прерывание ISR. В этом коде мы настроем контакт А левого и правого энкодеров как контакт прерывания; он вызывает ISR при появлении положительного импульса.
void SetupEncoders()
{
152 Согласование приводов и датчиков с контроллером робота
// Квадратурные энкодеры
// Левый энкодер
pinMode(Left_Encoder_PinA, INPUT_PULLUP);
// устанавливает pin A в качестве входа
pinMode(Left_Encoder_PinB, INPUT_PULLUP);
// устанавливает pin B в качестве входа
attachInterrupt(Left_Encoder_PinA, do_Left_Encoder, RISING);
// Правый энкодер
pinMode(Right_Encoder_PinA, INPUT_PULLUP);
// устанавливает pin A в качестве входа
pinMode(Right_Encoder_PinB, INPUT_PULLUP);
// устанавливает pin B в качестве входа
attachInterrupt(Right_Encoder_PinA, do_Right_Encoder, RISING);
}
Следующая часть кода является встроенной функцией loop() на языке wiring. Функция loop() – это бесконечный цикл, в который мы помещаем основной код. В этом коде мы вызываем функцию Update_Encoders() для непрерывной публикации значения энкодера через последовательный терминал (serial terminal).
void loop()
{
Update_Encoders();
}
Следующий код является определением функции Update_Encoders(). Он публикует два значения энкодера в строке со стартовым символом e, и значения разделяются пробелами табуляции. Функция Serial.print() – это встроенная функция, которая выведет заданную в качестве аргумента символ/строку.
void Update_Encoders()
{
Serial.print(«e»);
Serial.print(«\t»);
Serial.print(Left_Encoder_Ticks);
Serial.print(«\t»);
Serial.print(Right_Encoder_Ticks);
Serial.print(«\n»);
}
Следующий код является определением ISR левого и правого кодировщиков. Когда на каждом из выводов будет обнаружен положительный фронт импульса, будет вызвана одна из ISR. Текущие контакты прерывания – Pin А каждого из энкодеров. После получения прерывания мы можем предположить, что возрастающее значение на контакте А имеет более высокое значение, поэтому считывать его нет необходимости. Читаем контакт B двух энкодеров и сохраняем состояние контактов как LeftEncoderBSet или RightEncoderBSet. Далее, после сравнения текущего состояния с предыдущим состоянием контакта В, мы можем определить направление вращения и решить, следует ли в соответствии с таблицей состояния перехода увеличить или уменьшить количество импульсов.
void do_Left_Encoder()
{
LeftEncoderBSet = digitalRead(Left_Encoder_PinB);
// Читаем данные с вводного пина
Left_Encoder_Ticks -= LeftEncoderBSet ? -1 : +1;
}
void do_Right_Encoder()
{
RightEncoderBSet = digitalRead(Right_Encoder_PinB);
// Читаем данные с вводного пина
Right_Encoder_Ticks += RightEncoderBSet ? -1 : +1;
}
Загрузите черновик кода и просмотрите выходные данные с помощью последовательного монитора в Energia. Откройте меню Tools → Serial monitor. Проверните валы обоих двигателей вручную, чтобы отобразить данные, генерируемые энкодерами. Установите скорость передачи данных в последовательном мониторе, совпадающую с инициализированной в коде, – 115 200.
Выходные данные будут выглядеть так:
Чтобы модернизировать робот для работы с высокой точностью и повышенной полезной нагрузкой, следует подумать о приводах высокого качества, таких как Dynamixel. Сервоприводы Dynamixel – это интеллектуальные приводы, которые имеют встроенное ПИД-регулирование и мониторинг параметров сервопривода и энкодера, таких как крутящий момент, положение и т. д.
Работа с приводом Dynamixel
Dynamixel – это исполнительные механизмы с возможностью подключения в общую шину управления, разработанные корейской фирмой ROBOTIS специально для нужд робототехники. Эти приводы многофункциональны, имеют возможность передавать управляющему компьютеру для контроля такие параметры, как положение, скорость, внутренняя температура, контроль входного напряжения и прочее (функция обратной связи).
Сервоприводы Dynamixel соединяются последовательно, друг за другом. Данный способ последовательного подключения позволяет контролировать все сервоприводы одним контроллером. Сервоприводы Dynamixel подключаются через RS485 или TTL. Список доступных сервоприводов Dynamixel находится по адресу: http://www.robotis.com/shop/.
Интерфейс Dynamixel очень простой. Dynamixel поставляется с контроллером под названием USB2Dyanmixel. Данный контроллер преобразует USB в уровни Dynamixel, совместимые с TTL/RS485. На следующем рисунке показано подключение схемы Dynamixel.
ROBOTIS предоставляет Dynamixel SDK доступ к регистрам двигателей; мы можем считывать и записывать значения в регистры Dynamixel и получать для контроля такие данные, как положение, температура, напряжение и прочее.
Инструкции по установке USB2Dynamixel и пакета Dynamixel SDK: support.robotis.com/en/.
Dynamixel программируется с помощью библиотек Python. Одна из библиотек Python для обработки данных сервоприводов Dynamixel – это pydynamixel. Этот пакет доступен для таких операционных систем, как Windows и Linux. Данная библиотека работает с сервоприводами серий RX, MX и EX.
Пакет Python pydynamixel можно скачать по ссылке https://pypi.python.org/pypi/dynamixel/.
Скачайте пакет и распакуйте его в домашнюю папку. Затем откройте Terminal и выполните следующую команду:
sudo python setup.py install
После установки пакета можно попробовать обнаружить сервопривод, USB2Dynamixel и описать случайную позицию серводвигателя. Для этого воспользуйтесь следующим примером, который написан для сервоприводов серии RX и MX:
#!/usr/bin/env python
Следующий код импортирует необходимые для этого примера модули Python и включает в себя модули Dynamixel Python:
import os
import dynamixel
import time
import random
Код, приведенный ниже, определяет основные параметры для связи c Dynamixel. Переменная nServos обозначает количество сервоприводов Dynamixel, подключенных к шине. Переменная portName указывает на последовательный порт USB2Dynamixel, к которому подключен сервопривод Dynamixel. Переменная baudRate – скорость передачи USB2Dynamixel и Dynamixel.
# Количество Dynamixels на нашей шине
nServos = 1
# Установите последовательный порт соответствующим образом
if os.name == «posix»:
portName = «/dev/ttyUSB0»
else:
portName = «COM6»
# Скорость передачи данных по умолчанию устройства USB2Dynamixel
baudRate = 1000000
Следующий код – это функция Dynamixel Python для подключения к сервоприводу Dynamixel. Когда сервопривод будет подключен, программа для обнаружения подключенных устройств будет сканировать сообщения шины и определит номер сервопривода, начиная с ID от 1 до 255. Идентификатор каждого сервопривода является его же идентификацией. Когда сервопривод будет идентифицирован nServos как 1, сканирование будет остановлено, и на шине данных будет зафиксировано одно устройство:
# Подключение к последовательному порту
print «Connecting to serial port», portName, ‘…’,
serial = dynamixel.serial_stream.SerialStream( port=portName,
baudrate=baudRate, timeout=1)
print «Connected!»
net = dynamixel.dynamixel_network.DynamixelNetwork( serial )
net.scan( 1, nServos )
Следующий код добавит идентификатор Dynamixel и сервопривод в список myActutors. Мы можем написать значения сервопривода, используя его ИД и сервопривод как объект. Мы можем использовать список myActuators для дальнейшей обработки:
# удерживать список dynamixels
myActuators = list()
print myActuators
Это действие создаст список для хранения данных, получаемых с приводов dynamixel.
print «Scanning for Dynamixels…»,
for dyn in net.get_dynamixels():
print dyn.id,
myActuators.append(net[dyn.id])
print «…Done»
Следующий код напишет случайные положения каждому доступному на шине данных сервоприводу Dynamixel в диапазоне от 450 до 600. Диапазон позиций в Dynamixel – от 0 до 1023, который позволит установить такие параметры сервопривода, как скорость (speed), крутящий момент (torque), диапазон крутящего момента (torque_limt), максимальное значение крутящего момента (max_torque) и т. д.:
# Установка скорости и крутящего момента по умолчанию
for actuator in myActuators:
actuator.moving_speed = 50
actuator.synchronized = True
actuator.torque_enable = True
actuator.torque_limit = 800
actuator.max_torque = 800
Следующий код будет публиковать текущую позицию в текущем приводе:
# Перемещайте сервоприводы случайным образом и публикуйте их текущие позиции
while True:
for actuator in myActuators:
actuator.goal_position = random.randrange(450, 600)
net.synchronize()
Работа с ультразвуковыми датчиками расстояния 157
Следующий код будет считывать все данные из приводов:
for actuator in myActuators:
actuator.read_all()
time.sleep(0.01)
for actuator in myActuators:
print actuator.cache[dynamixel.defs.REGISTER[‘Id’]], actuator.
cache[dynamixel.defs.REGISTER[‘CurrentPosition’]]
time.sleep(2)
Следующий код будет считывать все данные из приводов:
for actuator in myActuators:
actuator.read_all()
time.sleep(0.01)
for actuator in myActuators:
print actuator.cache[dynamixel.defs.REGISTER[‘Id’]], actuator.
cache[dynamixel.defs.REGISTER[‘CurrentPosition’]]
time.sleep(2)
Работа с ультразвуковыми датчиками расстояния
Как уже упоминалось ранее, основная характеристика разрабатываемого нами мобильного робота – это автономная навигация. Идеальная навигация означает, что робот может планировать свой путь от текущей позиции до места назначения и двигаться без каких-либо столкновений. Мы в этом роботе для обнаружения объектов в непосредственной близости используем ультразвуковые датчики расстояния. Эти объекты не могут быть обнаружены с помощью сенсора Kinect. Сочетание Kinect и ультразвуковых датчиков с высокой степенью вероятности позволяет избежать столкновения робота с препятствием.
Ультразвуковые датчики расстояния работают следующим образом. Передатчик излучает не слышный для человеческого уха ультразвуковой сигнал. После отправки ультразвуковой волны приемная часть ультразвукового датчика ждет отраженный от обнаруживаемого препятствия сигнал (эхо). Если отраженного сигнала нет, значит, перед роботом нет препятствий. Если ультразвуковой датчик принимает отраженный сигнал, значит, по курсу у робота появилось препятствие. Чем короче временной промежуток между излученным ультразвуковым сигналом и принятым от него эхом, тем меньше расстояние между роботом и препятствием. Зная этот временной промежуток, мы, используя следующую формулу, можем вычислить расстояние до препятствия:
Скорость звука × (Пройденное время / 2) = Расстояние до объекта.
Значение скорости звука мы принимаем равным 340 м/с.
Большинство ультразвуковых датчиков способно обнаружить препятствие на расстоянии от 2 до 400 см. В этом роботе мы используем датчик HC-SR04. Рассмотрим, как согласовать датчик HC-SR04 с микроконтроллером Tiva C Launchpad для измерения расстояния от робота до препятствий.
Согласование HC-SR04 с Tiva C LaunchPad
На следующем рисунке показана схема подключения ультразвукового датчика HC-SR04 к микроконтроллеру Tiva C LaunchPad.
Питание ультразвукового датчика расстояния осуществляется напряжением +5 В. Так как амплитуда входного и выходного сигналов датчика равна 5 В, нам для взаимодействия с LaunchPad потребуется преобразователь с 5 на 3,3 В. Контакты преобразователя 5 В подключаются к контактам датчика Trig и Echo. Контакты преобразователя 3,3 В подключаются к 9-му и 10-му контактам микроконтроллера (как подключить преобразователь к датчику, показано на предыдущем рисунке). Теперь, когда датчик подключен, рассмотрим программу, управляющую контактами ввода/вывода (I/O).
Работа HC-SR04
Временные диаграммы сигнала (осциллограммы) для каждого контакта показаны на следующем рисунке. Для запуска триггера нам необходимо подать на его вход (Trig input) короткий импульс в 10 мкс. Триггер будет запущен, и на его выходе (Integral signal) пакет из 8 импульсов, модулированных частотой 40 кГц. Этот пакет импульсов будет воспроизведен динамиком датчика и, превратившись в звуковую волну, начнет свой путь к препятствию. Отразившись от препятствия, эхо-сигнал вернется в приемную часть датчика (микрофон). Эхо-сигнал (отраженный сигнал) – это расстояние до объекта, пропорциональное ширине импульса и дистанции. Дистанция рассчитывается через временной интервал между отправкой триггерных сигналов и приемом эхо-сигналов по следующей формуле:
Дистанция = Время получения отраженного сигнала × Cкорость звука (340 м/с) / 2.
Чтобы избежать перекрытия между триггером и эхом, желательно перед новым запуском триггера использовать задержку по времени в 60 мс:
Получение данных через Tiva C LaunchPad
Приведенный ниже код Energia для микропроцессора LaunchPad считывает с последовательного порта значения с ультразвукового датчика. Далее назначает контакты в LaunchPad для обработки ультразвукового эхо-сигнала и запуск триггеров. Одновременно устанавливаются переменные длительности импульса и расстояния в сантиметрах:
const int echo = 9, Trig = 10;
long duration, cm;
Следующий фрагмент кода является функцией setup(). Функция setup() вызывается при запуске программы. Используйте эту функцию для инициализации переменных, режимов контактов, запуска библиотек и т. д. Функция настройки запускается только один раз после каждого включения или перезагрузки LaunchPad.
Функция setup() инициализирует связь через последовательный порт со скоростью передачи данных 115 200 бод и устанавливает ультразвуковой режим, настраивая контакты с помощью функции SetupUltrasonic():
void setup()
{
// Последовательный порт со скоростью передачи 115200 бод
Serial.begin(115200);
SetupUltrasonic();
}
Следующая функция назначает контакт Trigger как OUTPUT, а контакт Echo – в качестве INPUT. Для определения контактов в качестве INPUT или OUTPUT используется функция pinMode().
void SetupUltrasonic()
{
pinMode(Trig, OUTPUT);
pinMode(echo, INPUT);
}
После создания функции setup(), которая инициализирует и задает начальные значения, функция loop() оправдывая свое имя и создает последовательные циклы, позволяющие программе менять значения и передавать их. Используйте ее для полного контроля микропроцессора Launchpad.
Основной цикл находится в следующем коде. Эта функция представляет собой бесконечный цикл и вызывает функцию Update_Ultra_Sonic() для обновления и публикации значений ультразвука и считывания через последовательный порт:
void loop()
{
Update_Ultra_Sonic();
delay(200);
}
Следующий код является определением функции Update_Ultra_Sonic(). Эта функция выполняет следующие операции. Во-первых, она принимает значение сигнала пуска — LOW длительностью 2 мс и HIGH длительностью 10 мс. После 10 мс функция снова возвратит значение контакта к значению LOW. Все эти преобразования выполняются согласно временной диаграмме. Мы уже видели, что ширина импульса запуска составляет 10 мкс.
После импульса запуска длительностью 10 мкс нам следует отследить время получения сигнала от контакта Echo, т. е. сколько было затрачено времени на прохождение звука от излучателя датчика до объекта и от объекта к приемнику датчика. Мы можем считать длительность импульса с помощью функции pulseIn(). После того как будет определено это значение (время, затраченное на прохождение ультразвуковой волны к препятствию и обратно), мы, как показано в следующем коде, можем преобразовать время в сантиметры с помощью функции microsecondsToCentimeters():
void Update_Ultra_Sonic()
{
digitalWrite(Trig, LOW);
delayMicroseconds(2);
digitalWrite(Trig, HIGH);
delayMicroseconds(10);
digitalWrite(Trig, LOW);
duration = pulseIn(echo, HIGH);
// перевести время в расстояние
cm = microsecondsToCentimeters(duration);
// Отправка через последовательный порт
Serial.print(«distance=»);
Serial.print(«\t»);
Serial.print(cm);
Serial.print(«\n»);
}
Следующий код является функцией преобразования из микросекунд в расстояние в сантиметрах. Скорость звука составляет 340 м/с, то есть 29 мс на сантиметр. Таким образом, мы получаем общее расстояние, разделив общее количество микросекунд на 29/2:
long microsecondsToCentimeters(long microseconds)
{
return microseconds / 29 / 2;
}
После загрузки кода с помощью команды меню Tools → Serial Monitor откройте окно последовательного монитора Energia и измените в правом нижнем открывающемся списке значение скорости на 115 200. После этого вы можете увидеть значения ультразвукового датчика, как показано на рисунке.
Согласование Tiva C LaunchPad с Python
В этой части статьи мы рассмотрим, как подключить Tiva C LaunchPad с Python для передачи данных из Launchpad на бортовой ПК. Модуль PySerial используется для взаимодействия Launchpad с Python.
PySerial можно установить с помощью менеджера пакетов Ubuntu без затруднений. Для установки используется следующая команда, введенная в терминале:
$ sudo apt-get install python-serial
После установки пакета python-serial мы можем написать код для интерфейса LaunchPad. Код взаимодействия приведен в следующей части. Этот код импортирует модули serial и sys. Модуль serial обрабатывает последовательные порты LaunchPad и выполняет операции чтения и передачи данных и т. д. Модуль sys предоставляет доступ к переменным, которые используются или поддерживаются интерпретатором и взаимодействующими с ними функциями. Они всегда доступны для всей программы.
#!/usr/bin/env python
import serial
import sys
Когда мы подключаем LaunchPad к компьютеру, устройство регистрируется в ОС как виртуальный последовательный порт. В Ubuntu название устройства будет /dev/ttyACMx, где x – это количество подключенных устройств. Если подключено только одно устройство, значение x будет равно 0. Для взаимодействия с LaunchPad мы должны обрабатывать информацию с этого устройства.
Следующий код попытается открыть последовательный порт /dev/ttyACM0 LaunchPad со скоростью передачи данных 115 200. Если это не удастся, на экране появится сообщение Unable to open serial port («Не удалось открыть последовательный порт»).
try:
ser = serial.Serial(‘/dev/ttyACM0’,115200)
except:
print «Unable to open serial port»
Следующий код будет считывать серийные данные до тех пор, пока серийный символ не перейдет на новую строку (‘\n’) и не опубликует ее в терминале. Если для выхода из программы нажать на клавиатуре сочетание клавиш Ctrl+C, будет вызван sys.exit(0).
while True:
try:
line = ser.readline()
print line
except:
print «Unable to read from device»
sys.exit(0)
После сохранения файла измените его разрешение на выполнение с помощью следующей команды:
$ sudo chmod +X script_name
$ ./script_name
Вывод скрипта будет выглядеть так:
Работа с ИК-датчиком расстояния
Принцип определения расстояния от робота до препятствия с помощью инфракрасного датчика отличается от принципа определения расстояния с помощью ультразвукового датчика. Здесь от препятствия отражается не ультразвук, а инфракрасный свет, излучаемый прожектором ИК-датчика. Приемник ИК-излучения захватывает отраженный инфракрасный свет. Напряжение на выходе приемника ИК-излучения напрямую зависит от уровня его освещенности.
Один из популярных датчиков ИК-диапазона – это Sharp GP2D12. Ссылка для приобретения этого датчика приведена ниже: http://www.robotshop.com/en/sharp-gp2y0a21yk0f-ir-range-sensor.html.
Альтернатива на AliExpress: https://ru.aliexpress.com/wholesalecatId=0&initiative_id=SB_20171211211645&SearchText=Sharp+GP2Y0A21YK0F+IR.
На следующем рисунке показан датчик Sharp GP2D12.
Датчик посылает луч инфракрасного света и использует триангулирование (метод разбиения поверхности на треугольники) для измерения расстояния. Диапазон обнаружения GP2D12 составляет от 10 до 80 см. Ширина светового пятна на расстоянии 80 см составляет 6 см. Передача и отражение светового луча ИК-диапазона:
С левой стороны датчика находится прожектор инфракрасного излучения, который непрерывно посылает свет в ИК-диапазоне. После попадания на поверхность препятствия свет отражается и фиксируется объективом приемника ИК-излучения. Схема подключения инфракрасного датчика показана ниже.
Аналоговый контакт Vo подключается к контакту АЦП (ADC) LaunchPad. Код взаимодействия датчика расстояния Sharp с микроконтроллером Tiva C LauncPhad будет рассмотрен в этом разделе. В этом коде мы назначим контакту 18 LaunchPad режим АЦП и будем считывать уровни напряжения от датчика расстояния Sharp. Уравнение диапазона ИК-датчика GP2D12 определяется следующим образом:
Range = (6787 / (Volt – 3)) – 4.
Здесь Volt – значение аналогового напряжения от контакта АЦП Vout. В первом разделе кода в качестве входных данных задается 18-й контакт Tiva C LaunchPad и запускается последовательная связь со скоростью передачи данных 115 200:
int IR_SENSOR = 18; // Датчик подключён к аналогову входу
int intSensorResult = 0; // Результат от датчика
float fltSensorCalc = 0; // Расчётное значение
void setup()
{
Serial.begin(115200); // Установки связи с компьютером для представления результатов
// через последовательный порт
}
В следующем разделе кода контроллер постоянно считывает данные с аналогового вывода и преобразует его в значение расстояния в сантиметрах:
void loop()
{
// Прочитать значения от датчика ИК
intSensorResult = analogRead(IR_SENSOR); //Получить значение датчика
// Рассчитать расстояние в см по уравнению диапазона
fltSensorCalc = (6787.0 / (intSensorResult – 3.0)) – 4.0;
Serial.print(fltSensorCalc); //Отправить расстояние на компьютер
Serial.println(» cm»); //Добавить см к результату
delay(200); //Ожидание
}
Это основной код интерфейса датчика расстояния Sharp. Но инфракрасным датчикам расстояния присущи некоторые недостатки:
- мы не можем использовать эти датчики при солнечном свете. Поэтому работа такого робота вне помещения затруднена;
- если препятствие не отражает инфракрасный свет, датчик не работает;
- датчик работает только в пределах ИК-диапазона.
В следующей части мы обсудим работу IMU (инерционного измерительного блока) и его взаимодействие с микропроцессором Tiva C Launchpad. IMU передает данные одометрии, и его можно использовать как передатчик входных сигналов в алгоритмах навигации.
Работа с инерционным измерительным модулем
Инерциальный измерительный модуль (IMU) – это электронное устройство, которое с помощью комбинации акселерометра, гироскопа и магнитометра измеряет скорость, ориентацию и гравитационные силы. IMU в робототехнике можно применять для решения многих задач. Например, это балансировка беспилотных летательных аппаратов (БПЛА) и навигация роботов.
В этой части статьи мы обсудим применение IMU в мобильной робототехнической навигации и познакомимся с несколькими моделями инерциальных блоков, предлагаемыми на рынке, рассмотрим их взаимодействие с LaunchPad.
Инерциальная навигация
IMU определяет ускорение и ориентацию робота по отношению к инерциальному пространству. Если мы знаем начальное положение, скорость и направление, можно вычислить скорость. По начальному положению, ускорению и скорости можно получить значение текущего положения. Для определения требуемого направления робота нам понадобится информация по ориентации робота. Эту информацию мы можем получить путем интеграции значений угловой скорости от гироскопа.
На следующем рисунке показана инерциальная навигационная система, которая преобразует значения IMU в данные одометрии.
Значения, полученные из IMU, преобразуются в навигационную информацию с помощью навигационных уравнений с применением оценочных фильтров, таких как фильтр Кальмана. Фильтр Кальмана – это алгоритм, который оценивает состояние системы от полученных данных измерений (http://en.wikipedia.org/wiki/Kalman_filter). Данные инерциальной системы навигации (ins) имеют погрешность, источник которой – ошибки измерений акселерометра и гироскопа. Для уменьшения значения погрешности ins используют данные, получаемые от других датчиков, обеспечивающие прямые измерения измеряемых величин. Основываясь на данных измерения датчика, модели ошибки датчика и фильтра Калмана, оцениваются ошибки в навигационных уравнениях и все ошибки остальных датчиков. На следующей схеме показана инерциальная навигационная система с использованием фильтра Калмана.
Вместе с данными от энкодера значение от IMU принимается как значение одометра, и его можно использовать для вычисления текущей позиции движущегося объекта с использованием ранее определенного положения. В следующей части статьи познакомимся с наиболее популярным инерциальным измерителем от InvenSense, который называется MPU 6050.
Взаимодействие MPU 6050 с Tiva C LaunchPad
Семейство MPU-6000/MPU-6050 – это первые и единственные в мире 6-осевые устройства с низкой мощностью потребления, отслеживающие движение. Устройства имеют низкую цену, высокую эффективность и соответствуют требованиям IMU смартфонов, планшетов, носимых датчиков и роботов.
Устройства МПУ-6000/6050 сочетают в себе 3-осевой гироскоп и 3-осевой акселерометр на кремниевом кристалле, соединенный с бортовым цифровым процессором движения, который может вести комплексную обработку сложных 9-осевых алгоритмов объединения движения. На следующей блок-схеме показаны схема MPU 6050 и передача данных MPU 6050.
Устройство MPU 6050 показано на следующем рисунке. Интегральный измерительный блок можно приобрести по адресу: https://www.sparkfun.com/products/11028.
Альтернатива на AliExpress: https://aliexpress.ru/item/32998950813.html?spm=a2g0o.productlist.0.0.7dc03e42WOyHn8&algo_pvid=38e25676-3761-442e-961e-0b26056ad6b1&algo_expid=38e25676-3761-442e-961e-0b26056ad6b1-6&btsid=0b8b15ea16089211444578360e7c23&ws_ab_test=searchweb0_0,searchweb201602_,searchweb201603_&sku_id=67014364931.
Подключение MPU 6050 к LaunchPad приведено в следующей таблице. Остальные контакты не подключаются.
Далее показана схема подключения MPU 6050 к Tiva C LaunchPad:
MPU 6050 и Launchpad взаимодействуют с помощью протокола I2C, питание – 3,3 В, получаемое от LaunchPad.
Установка библиотеки MPU 6050 в Energia
В этой части рассматривается код, позволяющий взаимодействовать с Energia. Код взаимодействия использует библиотеку для взаимодействия с MPU 6050 https://github.com/jrowberg/i2cdevlib/zipball/master.
Скачайте zip-файл по указанной ссылке. Откройте в Energia меню File → Preference, как показано на следующем рисунке.
В поле ввода Sketchbook location введите путь /home/имя_пользователя/Sketchbook, как показано на предыдущем рисунке, и создайте папку с именем библиотеки. Извлеките файлы из папки Arduino (zip-файл) в местоположение sketchbook/libraries. Пакеты Arduino этого репозитория совместимы с LaunchPad. Извлеченные файлы содержат следующие пакеты: I2Cdev, Wire и MPU6050. Эти пакеты необходимы для взаимодействия с датчиками MPU 6050. В папке библиотеки присутствуют и другие пакеты датчиков, но сейчас мы их использовать не будем.
Предыдущая инструкция предназначена для Ubuntu, но может быть выполнена в Windows и Mac OS X.
Код согласования в Energia
Этот код используется для чтения и передачи из MPU 6050 в LaunchPad необработанных значений. Код использует стороннюю библиотеку МПУ 6050, совместимую с Energia IDE. Ниже приведены объяснения каждого блока кода.
В этом первом разделе кода мы включаем заголовки, необходимые для взаимодействия MPU 6050 с Launchpad. Например, I2C, Wire и библиотека MPU6050, которая создает из MPU6050 объект с именем accelgyro. Библиотека MPU6050.h содержит класс, называемый MPU6050, предназначенный для отправки и получения данных от датчика:
#include «Wire.h»
#include «I2Cdev.h»
#include «MPU6050.h»
MPU6050 accelgyro;
В следующей части мы запускаем I2C и последовательную связь с MPU 6050. В результате датчик начнет передавать значения через последовательный порт. Скорость последовательной передачи составляет 115 200 бод. Setup_ MPU6050() – это пользовательская функция инициализации связи MPU 6050:
void setup()
{
// Последовательный порт со скоростью передачи 115 200 бод
Serial.begin(115200);
Setup_MPU6050();
}
Следующий раздел является определением функции Setup_MPU6050(). Библиотека Wire позволяет взаимодействовать с устройствами через I2C. MPU 6050 использует I2C для передачи данных. Функция Wire.begin() запустит связь I2C между MPU6050 и launchPad. Кроме того, данная функция инициализирует устройство MPU 6050 с помощью метода initialize(), определенного в классе MPU6050. Если все пройдет успешно, то на экране появится сообщение connection successful («соединение успешно установлено»). В противном случае вы увидите сообщение connection failed («связь прервана»):
void Setup_MPU6050()
{
Wire.begin();
// инициализация устройства
Serial.println(«Initializing I2C devices…»);
accelgyro.initialize();
// проверка соединения
Serial.println(«Testing device connections…»);
Serial.println(accelgyro.testConnection() ? «MPU6050 connection
successful» : «MPU6050 connection failed»);
}
Следующий код-функция loop() непрерывно считывает значение датчика и публикует значения через последовательный порт. Функция Update_MPU6050(), ответственная за публикацию обновленного значения от MPU 6050:
void loop()
{
// Обновление данных датчика MPU 6050
Update_MPU6050();
}
Update_MPU6050() определяется следующим образом. Функция объявляет шесть переменных для обработки значений по трем осям акселерометра и гироскопа. Функция getMotion6() в классе MPU 6050 отвечает за считывание новых значений с датчика. После прочтения он будет публиковать данные через последовательный порт:
void Update_MPU6050()
{
int16_t ax, ay, az;
int16_t gx, gy, gz;
// читать accel/gyro измерений от устройства
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// вывод значений accel/gyro x/y/z по отдельности
Serial.print(«i»);Serial.print(«\t»);
Serial.print(ax); Serial.print(«\t»);
Serial.print(ay); Serial.print(«\t»);
Serial.print(az); Serial.print(«\t»);
Serial.print(gx); Serial.print(«\t»);
Serial.print(gy); Serial.print(«\t»);
Serial.println(gz);
Serial.print(«\n»);
}
Вывод данных из последовательного монитора показан здесь:
Мы можем прочитать эти значения с помощью кода, написанного на Python. Этот код мы использовали для ультразвукового датчика. Ниже приведен снимок экрана терминала при запуске скрипта Python.
Итоги
В этой статье мы обсудили взаимодействие с двигателями, которые были использованы в нашем роботе. Также было рассмотрено взаимодействие двигателя и энкодера с контроллером Tiva C LaunchPad. Обсудили код контроллера, позволяющий подключить датчики и двигатели. В дальнейшем, если для робота потребуются повышенная точность и усиленный крутящий момент, обратитесь к сервоприводам Dynamixel, которые могут заменить двигатели постоянного тока. Также мы обсудили применение роботизированных датчиков, которые были использованы в нашем роботе. Были рассмотрены ультразвуковые датчики расстояния, инфракрасные датчики расстояния и инерциальный блок (IMU). Эти три датчика помогают роботу ориентироваться в помещении. Мы также обсудили основной код, позволяющий подключить эти датчики к микроконтроллеру Tiva C LaunchPad. Далее, в следующей статье «Согласование датчиков зрения с ROS«, мы уделим больше внимания взаимодействию с устройствами видеобзора и аудиодатчиками и использованию для этих целей Python.
За основу данной статьи взята книга Д. Лентина «Изучение робототехники с использованием Python»
С Уважением, МониторБанк