Ардуино гироскоп: варианты подключения, библиотеки, код

101 применение шлюза uart-to-i2c/spi/1w (rh-0004). часть 6. подключение 3-осевого гироскопа/акселерометра mpu6050 к компьютеру —

Drv8825

В качестве драйверов используются недорогие хорошо зарекомендовавшие себя драйверы DRV8825

На рисунке название ряда контактов сверху имеют линию, это знак «Инверсии» сигнала, он означает, что активный сигнал на данной линии – отрицательный:

• ENABLE – включение управления мотором (если 1 – выключен, 0 – включен);
• М0,М1,М2 – задают режим деления шага (дробления шага) см.таблицу 3.1;
• RESET – перезагрузка (в рабочем состоянии мотора должен быть подтянут к питанию логики 3.3в-5в);
• SLEEP – сон (в рабочем состоянии мотора должен быть подтянут к питанию логики 3.3в-5в);
• STEP – шаг, импульс на данном входе, это команда мотору сделать шаг;
• DIR – задает направление вращения вала мотора (само направление условно и зависит от порядка подключения обмоток);
• VMOT – ( ) контакт для подачи напряжения питания положительный;
• GNDMOT – (–/GND) контакт для подачи напряжения питания – (ЗЕМЛЯ);
• B1 и B2 – разъем для подключение первой обмотки (номера обмоток и номера контактов в обмотках условны);
• A1 и A2 – разъем для подключение второй обмотки (номера обмоток и номера контактов в обмотках условны);
• FAULT – выход сигнала аварии (действие не изучено, возможно устанавливается в низкий уровень при превышении нагрузки, замыкании или обрыве обмотки мотора);
• GNDLOGIC – GND (земля) логической части (контроллера управления), если источники питания логики и силовой части различны и не имеют общей «земли», если источник питания один, данный контакт можно не подключать.

Некоторые современные драйверы шаговых моторов могут дробить шаги на более мелкие, за счет плавного изменения тока обмоток. DRV8825 может делить шаг на 32 микрошага, таким микрошагом мы пользоваться не будем, но возможно воспользуемся данной возможностью для дробления шага до 1/16 (в роботе дробление шага 1/4), это увеличит плавность хода, что важно в нашем случае важно.

На драйвере есть подстроечный резистор (он находится под небольшим напряжением), он нужен для настройки тока фазы.

Для подстройки тока фазы требуется мультиметр и тоненькая плоская отвертка. На мультиметре устанавливаем режим измерения напряжения (до 2-5 вольт). Подключаем драйвер к силовому питанию. Измеряем напряжение между GND и металлической вращающейся головкой подстроечного резистора.

Измерив на подключенном к питанию драйвере напряжение между резистором и GND, и умножив полученное значение на 2, получим ток фазы. В нашем случае следует добиться значения в пределах 0.7-1.0В, что соответствует току через обмотки мотора 1.4-2А.

Большие значения поставить можно, но это приведет к нагреву драйвера и мотора, а также ускорит разряд батареи робота. Меньшие значения могут стать причиной того, что робот под нагрузкой (в режиме управления роботом) не будет поворачивать колеса или будет «пропускать» шаги. Вращаем головку (без усилий — можно повредить резистор), добиваемся нужного значения.

Для защиты драйвера от перегрева следует использовать радиатор (он идет в комплекте с драйвером). Радиатор снабжен клейкой основой. Снимите бумагу с клейкой части радиатора. Установите радиатор на микросхему драйвера, прижмите, произведите вращательные движения на 5-10 градусов, чтобы удалить пузыри воздуха между радиатором и микросхемой.

Значение микрошага шагового мотора в зависимости от подключения контактов M0,M1,M2 драйвера DRV8825.

Motionapps

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

MotionApps

. Это некий бинарный код, который записывается в память датчика. Код записывается в энергозависимую память, поэтому его нужно записывать каждый раз после подачи питания. Это занимает около секунды. Код собирает и фильтрует показания со всех осей акселерометра и гироскопа. Данные складываются в буфер

FIFO

. Собственно, Вам остается дождаться готовности данных и считать буфер со всеми показаниями. Пример на Питоне, который я нашел на GiHub:

Акселерометр

Акселерометр служит для измерения ускорения. При помощи акселерометра можно измерять ускорение при движении и, используя простые формулы, определять положения тела в пространстве. Например, если знать начальную скорость и считывать ускорение в равные малые промежутки времени, несложно вычислить пройденное расстояние Рассмотрим это на примере прямолинейного движения:

где

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

где

Рисунок Нахождение угла отклонения от вертикали (робот неподвижен)

Если у вас BNO055 используйте скетч BNO055test для тестирования гироприбора.

Визуализация

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

pygame

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

Успехов.

Смотри также:

Гироскоп

Гироскоп измеряет угловые скорости по трем осям с разными пределами измерений: 250, 500, 1000, и 2000 градусов в секунду. Пределы измерения могут быть выставлены в соответствии с Вашими задачами. Не стоит устанавливать без необходимости максимальные пределы, если у вас достаточно медленная система. Правильно выбранные пределы повысят точность измерений.

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

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

При этом мы предполагаем, что между двумя опросами датчика система поворачивалась с постоянной скоростью. Такой подход ведет к неизбежному накоплению ошибки. Что видно на примере. Смотри работу скрипта: mpu5060_gyro.py  или скрипта, графически отображающего положение гироскопа pyplay_gyro.py.

Гироскоп, это прибор способный реагировать на изменение ориентации объекта. простейшим примером гироскопа является волчок. если волчок раскрутить на перемещаемой поверхности, например, на доске, а затем наклонять эту доску, то волчок будет пытаться сохранить свою ориентацию.

Гироскоп был изобретен в начале XIX века, и в конце того же века был впервые применен на практике для стабилизации курса торпед.

На рисунке показано, как можно измерить отклонение машинки от первоначального направления с помощью гироскопа. Черная стрелка – вектор движения, штрихованная – показания гироскопа. В позиции (б) машинка завернула, но стрелка гироскопа осталась на месте, она указывает на заданное при включении гироскопа направление.

Изменяем значения gpio

Если вы для своего робота используете не те GPIO, что прописаны в схемах и программе, их можно заменить в файле  step_motor.h

#define STEP_L 15 //Пин левого шага 

#define STEP_R 4 //Пин правого шага

#define STEPER_EN 2 //Пин врключения моторов

#define DIR_L 13 //Пин направления левого шага 

#define DIR_R 16 //Пин направления правого шага

Изменяем направление вращения колес

Если колеса по факту вращаются не в ту сторону, следует поменять  в файле step_motor.h значения указанных ниже констант на противоположные:

#define DIR_L_FRONT false //Пин направления левого шага 

#define DIR_R_FRONT true //Пин направления правого шага

#define DIR_L_BACK true //Пин направления левого шага 

#define DIR_R_BACK false //Пин направления правого шага

Исследуем работу bno055

Если у вас BNO055 используйте скетчBNO055test для тестирования гироприбора.

1. Прошейте робота данной программой.

2. В Arduino IDE откройте «Монитор порта», установите скорость обмена 115200, как показано на следующем рисунке.

Теперь в мониторе порта вы увидите несколько строк данных, которые будут обновляться каждые 0.3 сек (delay(300)).
Первая строка содержит углы склонения/поворота робота. Вторая строка содержит угловую скорость по осям. Оси ориентации и угловой скорости могут не совпадать по названиям, какие оси использовать и с каким знаком следует установить.

Углы склонения/поворота робота

Покрутите робота, наклоните робота вперед/назад, вправо/влево.

event.orientation.x= ?? покажет угол поворота/налона робота в градусах по (вокруг) оси X прибора, event.orientation.y= ?? по оси Y, event.orientation.z= ?? по оси Z.

Установите робота параллельно горизонту и вращайте без наклонов. Заметьте — значения по какой оси изменяются — исключите эту ось — она нам пока не потребуется. Наклоните робота вправо/влево, запомните значения по какой оси будут в градусах соответствовать наклону, запомните эту ось, она нам не потребуется.

Нам нужна ось значения которой будут изменяться при наклонах робота вперед/назад, запомните эту ось, а также как будут изменяться значения угла — какой знак будет иметь угол поворота при наклоне вперед. Если этот знак будет отрицательным в программе балансировки у полученного значение угла потребуется менять знак (инвертировать).

Угловая скорость

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

Gyro.x()= ?? Gyro.y()= ?? Gyro.z()= ??

Вращайте робота в горизонтальной плоскости.

ВНИМАНИЕ, значение угловой скорости нужно снимать именно во время движения!

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

Наклоняйте робота вправо влево. Запомните ось, значения по которой будут значительно изменяться, они должны иметь разные знаки при повороте робота вправо и влево. Эту ось мы не должны использовать.

Наклоняйте робота вперед – назад. Запомните ось, значения по которой будут значительно изменяться, они должны иметь разные знаки при действии наклонения вперед – назад.

Еще раз отмечу, берите во внимание только показания, которые получаются, когда действие склонения робота производится!!! Когда робот остановлен, значение угловой скорости будут близки нулю и колебаться по знаку, он нам не интересны.

Эту ось мы будем использовать.

Если при действии «наклон вперед» угловая скорость по оси имеет отрицательный знак, в дальнейшем требуется менять знак (инвертировать).

Исследуем работу mpu6050

BNO055 сложный прибор, который сам производит промежуточные расчеты и подает на выход результат готовый к использованию, MPU6050 — простой прибор, который показывает только угловую скорость и довольно зашумленные показания акселерометра. Соответственно ряд расчетов потребуется проводить в программе. Рассмотрим, какие расчеты требуются.

Получение угла отклонения

MPU6050 не показывает угол отклонения от заданного направления. Прибор, регистрирует текущую угловую скорость (скорость вращения), и значения влияния гравитации по осям (акселерометр). Зная мгновенную угловую скорость, можно вычислить угол, на который повернется объект за некоторый промежуток времени:

где

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

Калибровка

Если считать данные с датчика можно заметить некоторую погрешность. Например, при абсолютном покое датчика показания гироскопа будут отличаться от нуля.
А показания акселерометра по модулю могут превышать 1. См. работу скрипта

mpu6050_get_raw_no_calibr.py

. Дело в том, что каждый экземпляр датчика требует калибровки. У некоторых цифровых датчиков можно встретить корректировочные коэффициенты, которые для каждого экземпляра прошивают в память датчика на этапе производства. К сожалению, в этом датчике такого нет, и мы сами должны позаботиться о калибровке.

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

В примере я предусмотрел процедуру калибровки. Скрипт калибровки: mpu6050_calibr.py. Калибровка выполняется в два этапа. Сначала датчик должен быть неподвижен. Подразумевается, что угловые скорости равны нулю. Калибруется гироскоп. Затем калибруется акселерометр. Нужно его не спеша поворачивать во всех направлениях. Эта процедура занимает около минуты. После чего функция калибровки выдает вычисленные значения, которые нужно прописать в скрипте. На этом калибровка закончена. После калибровки погрешность значительно снизится. См. Скрипт: mpu6050_get_raw.py. В этом скрипте в следующих строках указываются калибровочные данные:

mpu.gyro_offs = {`x`: -178, `y`: 259, `z`: -104}
mpu.accel_offs =  {`y`: -354, `x`: 389, `z`: -1482}

Комплектующие

Создаётся данный датчик или МК, в зависимости от того, что вы собрались приобретать, из компонентов ATmega328.

Так, в нём имеются:

1. 14 штук различных пинов и цифровых выходов, половина из которых являются ШИМ-выходами.
2. Специальные кварцевые резонаторы до 16 МГц мощностью.
3. Встроенный вход под usb-кабель, который позволит вам сэкономить не только время, но и деньги, которые вы могли бы потратить на покупку адаптера.
4. Контакты и распиновка для стандартного питания с нулем, фазой и заземлением.
5. Контакты для сброса до заводских настроек, при которых весь машинный код и данные будут стёрты. Это полезно в том случае, если вы напортачили с программой и модуль превратился в бесполезную груду железа, и просто как экономия времени, если необходимо сменить прошивку.
6. ICSP контакт, который необходим для того, чтобы запрограммировать машинный код, который будет находиться внутри системы.

Все эти компоненты и составляют Arduino гироскоп, позволяя ему выполнять свои базовые функции. Но как же запрограммировать систему, если вы до этого не имели опыта работа с данными МК?

Комплементарный фильтр

При помощи угловой скорости невозможно измерить угол склонения робота, можно измерить только изменение этого угла, да и то постоянно будет накапливаться погрешность. В теории нам было бы достаточно показаний акселерометра, чтобы получить склонение робота вперед/назад или вправо/влево но акселерометр MPU6050 сильно шумит, его показания довольно сильно изменяются, хотя и вокруг одного значения.

Ниже приведен пример измерения угла склонения робота по показаниям акселерометра. Я наклонял робота плавно, но показания довольно сильно пляшут.

Но на помощь нам приходит комплементарный фильтр! В нем смешиваются в определенных пропорциях показания по гироскопу и акселерометру, что позволяет получать довольно точную картину наклона робота. К сожалению такой фильтр невозможно применить при повороте робота в горизонтальной плоскости — не будет задействованы показания гравитации в акселерометре.

При объединение показаний акселерометра и гироскопа на основе комплементарного фильтра можно рассчитать значение угла наклона по формуле:

где .

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

Доля показаний акселерометра мала, поскольку они умножаются на малую величину

Но благодаря акселерометру, компенсируются ошибки измерений, которые обязательно возникают и накапливаются, если пользоваться только гироскопом..

Коэффициент

Благодаря использованию комплементарного фильтра, который математически намного проще, чем фильтр Калмана, удалось добиться стабилизации робота, и он успешно справляется с задачей балансировки.

Цитирование «Мобильные роботы на базе Arduino» закончено.

Используйте скетч MPU6050test для тестирования гироприбора.

1. Прошейте робота данной программой.

2. В Arduino IDE откройте «Монитор порта», установите скорость обмена 115200, как показано на следующем рисунке.

3. 
   Перезагрузите ESP32 кнопокой «Reset», не трогайте робота в течении 4х секунд, в это время производится расчет смещения показаний акселерометра.

Теперь в мониторе порта вы увидите несколько строк данных, которые будут обновляться каждые 0.1 сек .

Gyro =показания угловой скорости в радианах.

Acsel= показания угла наклона по акселерометру в радианах.

AcYsum =показания угла наклона Комплементарный фильтр.

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

Если при действии «наклон вперед» угловая скорость по оси имеет отрицательный знак, в дальнейшем требуется изменить знак (инвертировать). То же касается показаний угла Acsel. При проведении наклона вперед угловая скорость — положительна.

Запомним те изменения, которые внесены в типовой пример, они потребуются в полной программе балансировки.

Контроллер (управление)

В качестве контроллера использовался ESP32. Довольно интересный контроллер, который может быть запрограммирован из различных IDE, но мы будем использовать простую IDE Arduino, поддержку которой обеспечил производитель.

В последнее время я довольно активно использую данные контроллеры по причине высокой производительности и низкой стоимости, также стоит отметить широкий перечень возможностей, которые представляет ESP32. Это скорее микрокомпьютер, чем контроллер 512КБ ОЗУ, 4МБ Flash, два ядра по 240МГц, 64-разрядные таймеры и пр. и пр…

Назначение связки гироскоп и акселерометр

Для начала давайте разберёмся, зачем Arduino mpu 6050 (Gy-521) вообще нужен и что собой представляет гироскоп-акселерометр в целом. Такой датчик все мы видели в смартфонах, и там он выполняет следующие функции:

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

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

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

Всё зависит исключительно от вашей выдумки и конкретного проекта.

Чаще всего гироскоп для Ардуино применяется в системах автоматизации под так называемые «смартхаусы» (умные дома – прим. ред.), являясь своеобразным переключателем. Передавая определённые данные в МК, который затем отправляет их по блютуз-модулю к другому устройству, он может управлять всей техникой в доме.

Ещё один простой способ применения – использование вместо датчика движения на дверях, для включения света и кондиционирования, когда вы возвращаетесь домой.

Наладка

Далее наступает самый ответственный этап – отладка программного кода. Здесь вам необходимо подключить питание к прибору, а сам прибор – к компьютеру, чтобы следить за строками в консоли. Прогоните несколько базовых функций и посмотрите, не будет ли ошибок или багов. Если они возникают, то воспользуйтесь любым удобным методом дебагинга.

Самый простой вариант – использовать для ввода переменные, которые вычисляются рандомным образом, и смотреть, как код будет вести себя в различных ситуациях.

Общие сведения.

Основная микросхема модуль GY-521 чип MPU-6050 , который содержит в себе акселерометр и гироскоп, а так же датчик температуры. Обработка данных осуществляется с помощью 16-битное АЦП (Аналого-цифровой преобразователь) на каждый канал, поэтому он обрабатывает значение x, y и z одновременно.

Встроенный датчик температуры предназначен для измерения температуры и имеет диапазон измерений от -40 ° С до 85 ° С.Для взаимодействия с Arduino используется шина I2C и датчик MPU-6050 всегда выступает в качестве подчиненного устройства. Но кроме обычной шины I2C, есть собственный контроллер I2C, в котором MPU-6050 ведомый, выводы SDAи XDA и с помощью это шины можно управлять например магнитометром и передавать данные на Arduino.

Датчика MPU-6050 работает от напряжение ~2.4 — 3.5 В и чтобы стабилизировать питание, на модуле GY-521 добавили стабилизатор напряжения на 3.3 В с малым падением напряжении, поэтому модуль можно подключить к напряжению 5 В и 3.3 В.

Назначение выводов:►  VCC – « » питание модуля 3.3 В до 5 В ►  GND – «-» питание модуля►  SCL – линия синхронизации для протокола I2C►  SDA – линия передачи данных протокола I2C►  XDA – линия передачи данных протокола при работе в режиме мастера►  XCL – линия синхронизации для протокола I2C при работе в режиме мастера►  AD0 – если вывод лог «0» адрес I2C будет 0x68, если вывод лог «1» адрес I2C будет 0x69►  INT – прерывание

Подключение gy-521 к arduino

Необходимые детали:► Arduino UNO R3 x 1 шт.► Провод DuPont, 2,54 мм, 20 см x 1 шт.► Кабель USB 2.0 A-B x 1 шт.► 3-осевой гироскоп акселерометр GY-521 x 1 шт.

Подключение:В примере будем использовать только первые четыре контакта VCC, GND, SDA и SCL. Сначала подключаем VCC и GND модуля GY-521 к 5V и GND. Далее нужно установить соединение I2C между модулем GY-521 и Arduino, для этого подключите вывод SCL модуля к выводу А4 Arduino и вывод модуля SCL к выводу A5 Arduino. Для удобства приведу схему подключение.

Подключение гироскопа mpu6050 к esp32: схема и программа

Программы

Без программы модуль будет не более чем грудой железа, которая не выполнит ни одной функции. Базовые библиотеки для взаимодействия с другими МК можно найти на официальном сайте или в интернете, но, помимо них, вам потребуется вспомогательный код. С его помощью можно настроить взаимодействие между акселерометром и тем же блютуз модулем, без которого, в большинстве проектов, он станет бесполезен.

Мы воспользуемся готовой библиотекой для Arduino MPU 6050, которую написал Джефф Роуберг.

Прошивка

Я предполагаю, что вы знаете как устанавливать поддержку ESP32 в среду Arduino IDE. Я предполагаю, что вы знаете, как прошить ESP32. Замечу, что некоторые платы, например DEVKIT, требуют нажать кнопку BOOT для прошивки (во время прошивки), а затем кнопку RESET. Эти кнопки расположены на плате контроллера.

Сборка

Здесь всё зависит от используемого вами интерфейса, например, для I2C от Ардуино пригодятся контакты: A4, A5, которые являются SDA и SCL входами соответственно.

Для нормального функционирования всей этой системы необходимо будет использовать wire библиотеку в коде.

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

Питание модуля строго 3.3В!

Снимаем показания с гироприборов

(далее идет выборочное цитирование из моей книги «Мобильные роботы на базе Arduino»)

Совместное использование гироскопа и акселерометра

Использование отдельно акселерометра или отдельно гироскопа не даст желаемого результата, только их совместное использование с применением фильтрации с учетом показаний двух датчиков дает приемлемый результат. В примере реализован один из способов комплементарного фильтра.

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

См. работу скрипта mpu6050_final.py или скрипта pyplay_final.py, который графически изображает процесс работы MPU-6050

Схема подключения

Для подключения к контроллеру или микрокомпьютеру, у датчика имеется шина I

²

Схемы робота

Схемы робота приведены ниже. Также сборку робота можно проследить по роликам:

1 часть.

Теория и практика балансировки

По теоретической части управления обратным маятником существует довольно много литературы, с довольно красивыми математическими выкладками, но зачастую они не приведут вас к созданию эффективно работающего алгоритма балансировки.

Фактически при балансировке мы получаем колебательный контур,а колебания в контуре могут быть затухающими и незатухающими. Если наш робот колеблясь совершает каждый раз колебание с увеличивающейся амплитудой, то …. нужно что то подкрутить — изменить звенья обратной связи.

Хотя, пойдем от простого к сложному.

Все описанные далее формулы должны применяться к роботу периодически, через 5-10 миллисекунд.

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

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

Для точности нужно брать за основу пропорциональности синус угла склонения, но так как синус угла до 30 градусов отличается незначительно от своего значения в радианах, то для простоты и увеличения скорости обработки будем брать сам угол в радианах.

скорость ответа колес робота Speed должна быть пропорциональна углу склонения робота Ang. Это первая фаза построения регулятора.

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

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

Предположим, что мы узнали скорость падения робота, она может быть отрицательной и положительной. Также скорость падения может не совпадать с наклоном робота — робот наклонен в одну сторону, а благодаря компенсации колесами не падает, а поднимается.

Допустим ситуацию, когда робот склонён но уже не падает, а поднимается, логично предположить, что в этом случае робот должен хотя-бы остановиться, но он, «видя» свой наклон, продолжит ехать вперед, что приведет в обратному склонению (перерегулированию) и в дальнейшем к падению.

As – угловая скорость (скорость падения) , Kd —коэффициент пропорциональности.

Если применить данную формулу, то можно невооруженным глазом заметить, что балансировка стала плавнее. Для подбора Kd, примите Kp = 0, и добейтесь существенного противодействия падению одним вторым звеном. Робот будет падать но должен существенно этому сопротивляться.

Но еще один вопрос как быть, если робот уже движется с определенной скоростью и падает, где и как мы учитываем текущую линейную скорость робота? – нигде. А это желательно сделать!

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

где Speed_old – текущая скорость.

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

Настройка похожа на предыдущую: сначала принимаем Kd=0, добиваемся балансировки, затем Kp=0, также добиваемся существенного сопротивления робота падению, затем оба коэффициента снижаем вдвое от полученных значений и подключаем оба.

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

Вы уже задали вопрос — «почему робот не стоит на месте?» – пора задать!

Дело в том, что точно отцентрировать робота относительно гироприбора сложно и робот будет всегда немного наклонен вперед или назад, наклон задается смещением центра тяжести. Для компенсации этого наклона применяется обратная связь по пройденному расстоянию, а это не что иное как скорость умноженная на dt – время действие данной скорости. Так как скорость робота изменяется периодически, то следует использовать накопительную (интегральную) форму:

Move – пройденный роботом путь в относительных единицах (для шаговых моторов это могут быть шаги), dt- время действия скорости (период между опросами в секундах),

Ki —коэффициент пропорциональности.

Кажется все нормально, но на деле робот начинает колебаться с огромной амплитудой — изменяя Ki добейтесь этого. Дело в том, что работе интегральной составляющей Move нужно помогать останавливая робота за счет составляющей, которая уменьшается с уменьшением скорости робота, это как раз сама скорость робота — Speed, добавим ее небольшую часть для компенсации раскачивания робота интегральным звеном…

где Kki – коэффициент затухания для интегрального звена.

Теперь наш робот будет устойчиво стоять и даже если его толкнуть — вернет себе устойчивое равновесие. Но, он еще не может двигаться!

Для того чтобы робот двигался нужно компенсировать последствия его движения, а его движения будут оказывать влияние на звено с накоплением — интегральное звено и его компенсатор затухания.

Если принять заданную скорость движения за SpZ, то формула примет вид:

Вот теперь робот будет двигаться с указанной скоростью и не только по равнине, но и в гору и с горы!

Можно и далее сглаживать движение робота, введением звеньев высшего порядка, но их влияние, как показывает практика, не велико.

Тестирование

После отладки необходимо провести тестирование. В чём разница? При тестировании вы точно знаете, что программный код работает без лагов и багов, но вам необходимо убедиться, что в нём нет логических ошибок.

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

Технические параметры.

►  Основная микросхема: MPU-6050 ►  Напряжение питания: 3.3 и 5 В►  Режимы для акселерометра: ±2g, ±4g, ±6g, ±8g, ±16g;►  Режимы для гироскопа: ±250°, ±500°, ±1000°, ±2000°;►  Ширина шины IIC: 16 бит;

Учимся работать с гироприбором

Если вы уже собрали робота, то отключите его моторы, вынув положительный контакт (провода питания драйверов DRV8825), или другим способом, если робот не собран. Возьмите модуль ESP32 и гироприбор, далее будут показаны принципы работы с MPU6050 и BNO055. В любом случае гироприбор должен быть подключен согласно схемы, указанной выше и соответствующей вашему гироприбору.

У приборов MPU6050 и BNO055, которые были у меня под рукой, при установке их на макетную плату контакты SCL и SDA распаяны по разному (перекрещены).

Но все зависит от распайки чипа на плате, вы можете самостоятельно менять GPIO для ESP32 I2C в строке включения I2C Wire.

begin(SDA, SCL), здесь вместо SDA иSCL установите номера GPIO вашего подключения.

Строка Wire.begin() для BNO055 находится в файле Adafruit_BNO055.cpp , для MPU6050 – в файле gyro_acsel.h .

Ориентацию осей также рекомендую определять экспериментально, получая показания прибора.

Если у вас BNO055, который имеет несколько режимов работы интерфейса, переключите его в режим I2C интерфейса закоротив перемычки S0 (PS0) и S1 (PS1) на GND, на рисунке с GY-BNO055 перемычки уже впаяны (рядом со стрелками осей).

Считаем, что гироприбор стоит на роботе, так как и будет стоять в дальнейшем. Сторону робота ближе к которой расположен гироприбор будем считать «FRONT», а соответственно ту, где расположены драйверы моторов и выключатель — «BACK», напишите маркером или сделайте метки маркером на роботе.

Шаговые моторы

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

Шаговые моторы должны иметь достаточную мощность для работы без перегрузок. Для этого подходят моторы Nema17 рассчитанные на ток 1.2-1.5А. Более мощные моторы будут избыточными, менее мощные не смогут без пропуска шагов перемещать достаточно тяжелую конструкцию робота.

Электропитание робота

В качестве источника питания для робота используется 4 литиевых аккумулятора формата 18650 подключенные последовательно, что дает напряжение 14-16 вольт и ток до 5А. Использование 4х аккумуляторов и столь высокого напряжения основано на требованиях двигателей к току (до 1.5А) и драйверов drv8825 к напряжению (не менее 9В).

Также потребуется один стабилизатор для уменьшения напряжения до 5В. Данное напряжение способен «переварить» внутренний стабилизатор платы ESP32 (сам контроллер использует напряжение 3.3вольта). Можно использовать линейный стабилизатор типа L7805CV или импульсный понижающий стабилизатор настроенный на выходное напряжение 5 вольт.

Ниже, в схеме робота будет приведено подключение линейного стабилизатора. Отмечу, что ножка 1 – ( входа), 2 — (GND), 3 – ( выходного напряжения 5вольт). Между обоими положительными контактами и GND устанавливаются гасящие помехи конденсаторы.

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