Общие принципы работы серводвигателей (сервомоторов)
Серводвигатели включают в свой состав небольшой двигатель постоянного тока, редуктор и схему управления, содержащую переменный резистор, дающий возможность установить выходной вал серводвигателя под определенным углом. Поэтому серводвигатели очень удобны для проектов, где требуется осуществлять весьма быстрое и относительно точное перемещение какого-либо рабочего органа.
Типы серводвигателей
Серводвигатели часто используются в радиоуправляемых моделях автомобилей для поворота рулевых колес или в моделях радиоуправляемых самолетов – для поворота управляющих поверхностей (рулей). На следующем рисунке показаны два серводвигателя разных размеров.
Серводвигатель справа представляет собой так называемый стандартный серводвигатель. Это наиболее распространенный тип серводвигателя. Такие серводвигатели достаточно часто имеют одинаковые размеры и монтажные расстояния между отверстиями. Намного меньший (и более легкий) серводвигатель слева предназначен для летательных аппаратов. Эти серводвигатели называются сервоприводами 9g .
Сервоприводы с более высоким качеством исполнения и более высоким крутящим моментом имеют редуктор с шестернями из металла, а не из нейлона. Большинство серводвигателей работают на номинальном напряжении питания около 5 В при допустимом диапазоне питающих напряжений от 4 до 7 В. Подключение любительских сервоприводов обычно осуществляется через провода, заканчивающиеся 3-контактным разъемом: питание +, питание — и управляющий сигнал.
Большие и иногда весьма мощные серводвигатели также доступны для использования, но они не так стандартизированы, как любительские маломощные сервомашинки.
Устройство сервопривода
Сервопривод (см. рисунок) состоит из электродвигателя, постоянного тока, приводящего в действие редуктор, уменьшающий скорость вращения двигателя и, в то же время увеличивающий крутящий момент на валу. Для контроля положения выходного вала он соединен с датчиком положения (как правило, это переменный резистор). Для управления мощностью и направлением, в котором поворачивается двигатель сервопривода, схема управления использует входной сигнал от датчика положения в сочетании с сигналом управления, задающим требуемое положение.
Блок управления, получив через сигнал управления величину желаемого положения вала, вычитает из него величину действительного его положения и вырабатывает «сигнал ошибки», который может быть положительным или отрицательным. Этот «сигнал ошибки» подается на питание двигателя, заставляя его изменить положение вала в нужном направлении. Чем больше разница между желаемым и действительным положением выходного вала, тем быстрее двигатель будет поворачиваться к желаемой позиции. Чем ближе к нулю становится значение ошибки (рассогласования), тем меньше становится питание двигателя.
Управление серводвигателем
Управляющий сигнал на серводвигатель — это не напряжение, как можно было бы ожидать, а сигнал широтно-импульсной модуляции (ШИМ). Этот сигнал является стандартным для всех любительских сервомашинок и выглядит так, как показано на следующем рисунке.
Серводвигатель ожидает прихода импульса управления каждые 20 мс. Импульс длительностью 1,5 мс установит серводвигатель в центральное положение, соответствующее повороту выходного вала на 90°. Более короткие импульсы в 1,0 мс установят выходной вал в начальное положение — 0°, а импульсы в 2,0 мс — крайнее положение — 180°. В реальности этот диапазон может быть немного меньше, чем полные 180°, без укорочения импульсов на одном конце и удлинения на другом. Не редкость и ситуация, когда для 0° нужен импульс 0,5 мс, а для 180° — 2,5 мс.
Назначение контактов сервомотора
Представлено на следующем рисунке. Я думаю, здесь все просто и понятно.
1. Red (красный) = Положительное напряжение питания (от 4.8V до 6V)
2. Brown (коричневый) = Ground (земля)
3. Orange (оранжевый) = Control Signal – управляющий сигнал (PWM Pin – контакт ШИМ)
Расчет угла с помощью гироскопа mpu6050
Данные с гироскопа имеют вид:
В дальнейшем в статье мы будем рассматривать все на примере оси x. Для расчета угла необходимо проинтегрировать переменную “gyro_x_scalled”
является количеством итераций
Так же стоит отметить, что на каждом временном промежутке цикла значение “gyro_x_scalled” остается одинаковым. Существует насколько подходов и методов интегрирования для компенсации и этой погрешности, но мы их детально не будем рассматривать.
Для реализации дискретного интегрирования, будем использовать метод Эйлера как один из самых популярных алгоритмов. Математически интегрирование методом Эйлера можно записать следующим образом:
Мы предполагаем, что начальные углы относительно осей x, y, z после калибровки равны 0, 0 и 90 градусов соответственно, так что для итерации n=0:
Значение T (время каждой итерации) и динамика самого гироскопа (как быстро и насколько нелинейно изменяются углы), значительным образом влияет на точность расчетов. Чем медленнее изменяются углы и чем меньше промежуток между итерациями, тем более точным будет результат. В этом смысле жаль, что платы Arduino достаточно медленные, кристаллы у них работают с частотой 16 МГц и снятие измерений каждые 10-20 мс становится достаточно затруднительным (учитывая тот факт, что процессор занят не только расчетом угла, но и другими параллельными задачами). Мы можем использовать T в виде переменной или константы, я, лично, предпочитаю использовать константу для каждого цикла. В проекте динамические факторы не учитывались, просто использовалась частота итераций с разрывом в 20 мс (0.02 с).
Подготовка приложения на Android
Приложение на Android для нашего проекта мы разработали с помощью бесплатной программной среды Processing IDE, программирование в которой очень похоже на программирование в Arduino. Ранее на нашем сайте программную среду Processing мы использовали в следующих проектах:
- игра в Ping Pong на основе Arduino;
- радар на Arduino;
- проект виртуальной реальности на Arduino.
В этом проекте вы можете скачать либо готовый APK файл приложения, не утруждая себя погружением в среду Processing, либо можете скачать исходные коды приложения если все таки хотите в нее погрузиться и изменить что либо в нашей программе.
Примечание: это приложение будет корректно работать если только Bluetooth устройство имеет имя “HC-06”. Чтобы установить приложение на смартфон разрешите в нем установку приложений из непроверенных источников.
Объяснение кода приложения для Android
Внутри скачанного ZIP файла вы найдете папку, в которой находятся все изображения и другие данные, необходимые для работы android приложения.
В следующей строке кода программы мы указываем с каким Bluetooth устройством необходимо автоматически соединиться приложению.
bt.connectToDeviceByName(«HC-06»);
| 1 | bt.connectToDeviceByName(«HC-06»); |
Внутри функции draw(), которая представляет собой бесконечный цикл, мы будем рисовать изображения и печатать текст на экране смартфона в зависимости от данных, принимаемых по Bluetooth.
void draw() //The infinite loop
{
background(0);
imageMode(CENTER);
image(logo, width/2, height/1.04, width, height/12);
load_images();
textfun();
getval();
}
|
1 |
voiddraw()//The infinite loop { background(); imageMode(CENTER); image(logo,width2,height1.04,width,height12); load_images(); textfun(); getval(); } |
Также помните о том, что в программе для Arduino мы разделили передаваемые данные на три части и поместили их все в диапазон от 0 до 255. Здесь же мы должны распаковать эти данные и вернуть им нормальные значения.
if (info<100 && info>0)
x = map(info, 0, 100, -(width/1.5)/3, +(width/1.5)/3);//x = info;
else if (info<200 && info>100)
y = map(info, 100, 200, -(width/4.5)/0.8, +(width/4.5)/0.8);//y = info;
else if (info>200)
temp = info -200;
println(temp,x,y);
|
1 |
if(info<100&&info>) x=map(info,,100,-(width1.5)3,+(width1.5)3);//x = info; elseif(info<200&&info>100) y=map(info,100,200,-(width4.5)0.8,+(width4.5)0.8);//y = info; elseif(info>200) temp=info-200; println(temp,x,y); |
Шаг 1. Компоненты для подключения акселерометра к Arduino
Для проекта понадобятся несколько компонентов:
Микроконтроллер Arduino UNO R3
МК создан с использованием материалов контроллера ATmega328:
- цифровые входы и выходы в количестве 14 штук, причем половина приходится на ШИМ-выходы;
- аналогичные входы, количество – 6 штук;
- резонатор на основе кварца, мощностью 16 МГц;
- встроен usb-вход;
- контакт для подключения питания;
- на МК располагается кнопка, с помощью которой возможен сброс данных и кода;
- контакт для программирования данных, находящихся внутри схемы, именуемый ICSP.
Старт работы начинается с подачи электрического питания в плату. Пользователь подключает к плате со схемой блок питания или зарядное устройство. Также процедура осуществляется с помощью usb-кабеля, который подключен к компьютеру и микроконтроллеру. Для разработки программы понадобится бесплатная среда программирования – Arduino IDE.
Внимание! Пользователь разрабатывает приложения с использованием Ардуино, только если платы совместимы с архитектурой микроконтроллера. В противном случае программа не заработает.. Пользователь создает в бесплатной среде код, затем его компилирует и загружает проработанную программу в пространство памяти в Ардуино
Язык, на котором программируется код, Wiring максимально приближен к популярному среди программистов языку – C++. Кроме того МК поддерживает версии для осей Виндовс, Мак ОС и Линукс
Пользователь создает в бесплатной среде код, затем его компилирует и загружает проработанную программу в пространство памяти в Ардуино. Язык, на котором программируется код, Wiring максимально приближен к популярному среди программистов языку – C++. Кроме того МК поддерживает версии для осей Виндовс, Мак ОС и Линукс.
Модуль датчика для гироскопа акселерометра на Аrduino с 3 осями – GY-521 (MPU-6050)
В основе компонента лежит микросхема MPU-6050. В комплект входят 2 предмета – гироскоп и акселерометр. Данные устройства перед конструированием обрабатываются и затем переносятся прямиком в микроконтроллер через интерфейс
Модуль датчика помогает определять место и перемещение инструмента в пространстве. Измеряются дифферент и углы крена посредством вектора силы тяжести и скорости в процессе вращения. Также включена функция измерения температурного режима. Перемещение определяется линейным ускорением и угловой скоростью. Полная картина рисуется по 3 осям.
Компонент нередко сравнивают с человеческим вестибулярным аппаратом, который помогает людям чувствовать силу тяготения и удерживать равновесие.
Макетная плата, предназначенная для прототипирования
Отладка – неотъемлемая часть построения электронных схем. Макетная плата незаменима для конструкции электронной аппаратуры. Ранее в изобретательстве использовали традиционные макетные платы, но сейчас широко распространены макетные платы, которые удобны тем, что не требуют дополнительных спаек.
Таким образом, процесс сборки и отладки электронной схемы в разы ускоряется: не приходится часто использовать паяльник, чтобы поменять сломанные радиодетали.
Материал для изготовления беспаечных макетных плат – пластик. Кроме того, все контакты надежно скреплены к плате, поэтому частые переключения не испортят элемент.
Соединительные провода папа-папа
Обычные провода папа-папа нам подойдут, еще их называют провода-перемычки. Такие стоят недорого и продаются везде, на любом рынке или в любом онлайн-магазине для радиолюбителей.
Исходный код программы на С
Arduino
#include «TinyWireM.h»
#include «TinyOzOLED.h»
int accelX, accelY, accelZ;
char mpu = 0x68;
float vectorprevious;
float vector;
float totalvector;
int Steps = 0;
void setup() {
TinyWireM.begin();
OzOled.init();
OzOled.clearDisplay();
OzOled.setNormalDisplay();
OzOled.sendCommand(0xA1); // установки ориентации дисплея
OzOled.sendCommand(0xC8);
TinyWireM.beginTransmission(mpu);
TinyWireM.write(0x6B); // установка регистра питания
TinyWireM.write(0b00000000); // отключаем спящий режим
TinyWireM.endTransmission();
TinyWireM.beginTransmission(mpu);
TinyWireM.write(0x1B); // конфигурация регистров гироскопа
TinyWireM.write(0x00000000); // 250° per second range (default)
TinyWireM.endTransmission();
TinyWireM.beginTransmission(mpu); //адрес I2C гироскопа
TinyWireM.write(0x1C); // конфигурация регистров акселерометра
TinyWireM.write(0b00000000); // 2g range +/- (default)
TinyWireM.endTransmission();
}
void loop() {
getAccel();
vector = sqrt( (accelX * accelX) + (accelY * accelY) + (accelZ * accelZ) );
//OzOled.printString(«Vec:», 0, 2);
//OzOled.printNumber(vector, 0, 5, 2);
totalvector = vector — vectorprevious;
//OzOled.printString(«Pre:», 0, 4);
//OzOled.printNumber(vectorprevious, 0, 5, 4);
//OzOled.printString(«Diff:», 0, 6);
//OzOled.printNumber(totalvector, 0, 5, 6);
if (totalvector > 6){
Steps++;
}
//String Step_count = String(Steps);
//char data;
//Step_count.toCharArray(data, 2);
//OzOled.printBigNumber(data, 6, 2, 3);
OzOled.printString(«Steps», 0, 4);
OzOled.printNumber(Steps, 0, 8, 4);
vectorprevious = vector;
delay(600);
//OzOled.clearDisplay();
}
void getAccel() {
TinyWireM.beginTransmission(mpu); //адрес I2C гироскопа
TinyWireM.write(0x3B); // регистр данных акселерометра
TinyWireM.endTransmission();
TinyWireM.requestFrom(mpu, 6); // считываем 6 байт, 2 for each DoF
accelX = TinyWireM.read() << 8|TinyWireM.read();
accelY = TinyWireM.read() << 8|TinyWireM.read();
accelZ = TinyWireM.read() << 8|TinyWireM.read();
// OzOled.printNumber(accelX, 0, 0, 0);
// OzOled.printNumber(accelY, 0, 0, 2);
//OzOled.printNumber(accelZ, 0, 0, 4);
}
|
1 |
#include «TinyWireM.h» intaccelX,accelY,accelZ; charmpu=0x68; floatvectorprevious; floatvector; floattotalvector; intSteps=; voidsetup(){ TinyWireM.begin(); OzOled.init(); OzOled.clearDisplay(); OzOled.setNormalDisplay(); OzOled.sendCommand(0xA1);// установки ориентации дисплея OzOled.sendCommand(0xC8); TinyWireM.beginTransmission(mpu); TinyWireM.write(0x6B);// установка регистра питания TinyWireM.write(0b00000000);// отключаем спящий режим TinyWireM.endTransmission(); TinyWireM.beginTransmission(mpu); TinyWireM.write(0x1B);// конфигурация регистров гироскопа TinyWireM.write(0x00000000);// 250° per second range (default) TinyWireM.endTransmission(); TinyWireM.beginTransmission(mpu);//адрес I2C гироскопа TinyWireM.write(0x1C);// конфигурация регистров акселерометра TinyWireM.write(0b00000000);// 2g range +/- (default) TinyWireM.endTransmission(); } voidloop(){ getAccel(); vector=sqrt((accelX*accelX)+(accelY*accelY)+(accelZ*accelZ)); //OzOled.printString(«Vec:», 0, 2); //OzOled.printNumber(vector, 0, 5, 2); totalvector=vector-vectorprevious; //OzOled.printString(«Pre:», 0, 4); //OzOled.printNumber(vectorprevious, 0, 5, 4); //OzOled.printString(«Diff:», 0, 6); //OzOled.printNumber(totalvector, 0, 5, 6); if(totalvector>6){ Steps++; } OzOled.printString(«Steps»,,4); OzOled.printNumber(Steps,,8,4); vectorprevious=vector; delay(600); //OzOled.clearDisplay(); voidgetAccel(){ TinyWireM.beginTransmission(mpu);//адрес I2C гироскопа TinyWireM.write(0x3B);// регистр данных акселерометра TinyWireM.endTransmission(); TinyWireM.requestFrom(mpu,6);// считываем 6 байт, 2 for each DoF accelX=TinyWireM.read()<<8|TinyWireM.read(); accelY=TinyWireM.read()<<8|TinyWireM.read(); accelZ=TinyWireM.read()<<8|TinyWireM.read(); // OzOled.printNumber(accelX, 0, 0, 0); // OzOled.printNumber(accelY, 0, 0, 2); //OzOled.printNumber(accelZ, 0, 0, 4); } |
Исходный код программы
Перед загрузкой кода программы в плату Arduino не забудьте скачать и установить библиотеку для работы с гироскопом MPU6050.
Arduino
/*
MPU6050 Library: https://github.com/jarzebski/Arduino-MPU6050 (c) 2014 by Korneliusz Jarzebski
*/
#include <Wire.h> //библиотека для связи по протоколу IIC
#include <MPU6050.h> //библиотека для работы с MPU6050
#include <SoftwareSerial.h>// библиотека последовательной связи
SoftwareSerial BT(10, 11); // RX, TX
MPU6050 mpu;
unsigned long timer = 0;
unsigned long timer2 = 0;
float timeStep = 0.01;
float pitch = 0;
float roll = 0;
float yaw = 0;
float temp =0;
void setup()
{
Serial.begin(115200);
BT.begin(9600); // инициализируем последовательную связь с модулем Bluetooth на скорости 9600 бод/с
// инициализируем MPU6050
while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println(«Could not find a valid MPU6050 sensor, check wiring!»);
delay(500);
}
mpu.calibrateGyro(); // старт калибровки гироскопа
mpu.setThreshold(3); // устанавливаем чувствительность гироскопа
}
void loop()
{
timer = millis();
//считываем данные температуры и гироскопа
Vector norm = mpu.readNormalizeGyro();
temp = mpu.readTemperature();
// рассчитываем показатели Pitch, Roll и Yaw
pitch = pitch + norm.YAxis * timeStep;
roll = roll + norm.XAxis * timeStep;
yaw = yaw + norm.ZAxis * timeStep;
// Print values
Serial.print(» Pitch = «);
Serial.print(pitch);
Serial.print(» Roll = «);
Serial.print(roll);
Serial.print(» Yaw = «);
Serial.print(yaw);
Serial.print(» Temp = «);
Serial.print(temp);
Serial.println(» *C»);
delay((timeStep*1000) — (millis() — timer)); // используем интервал 0.01 секунды для считывания данных
if ((millis()-timer2) > 200)
send_BT();
}
void send_BT()
{
int t;
int x;
int y;
if (roll>-100 && roll<100)
x = map (roll, -100, 100, 0, 100);
if (pitch>-100 && pitch<100)
y = map (pitch, -100, 100, 100, 200);
if (temp>0 && temp<50)
t = 200 + int(temp);
BT.write(x);
BT.write(y);
BT.write(t);
timer2 = millis();
}
|
1 |
/* SoftwareSerialBT(10,11);// RX, TX MPU6050mpu; unsignedlongtimer=; unsignedlongtimer2=; floattimeStep=0.01; floatpitch=; floatroll=; floatyaw=; floattemp=; voidsetup() { Serial.begin(115200); BT.begin(9600);// инициализируем последовательную связь с модулем Bluetooth на скорости 9600 бод/с // инициализируем MPU6050 while(!mpu.begin(MPU6050_SCALE_2000DPS,MPU6050_RANGE_2G)) { Serial.println(«Could not find a valid MPU6050 sensor, check wiring!»); delay(500); } mpu.calibrateGyro();// старт калибровки гироскопа mpu.setThreshold(3);// устанавливаем чувствительность гироскопа } voidloop() { timer=millis(); //считываем данные температуры и гироскопа Vectornorm=mpu.readNormalizeGyro(); temp=mpu.readTemperature(); // рассчитываем показатели Pitch, Roll и Yaw pitch=pitch+norm.YAxis*timeStep; roll=roll+norm.XAxis*timeStep; yaw=yaw+norm.ZAxis*timeStep; // Print values Serial.print(» Pitch = «); Serial.print(pitch); Serial.print(» Roll = «); Serial.print(roll); Serial.print(» Yaw = «); Serial.print(yaw); Serial.print(» Temp = «); Serial.print(temp); Serial.println(» *C»); delay((timeStep*1000)-(millis()-timer));// используем интервал 0.01 секунды для считывания данных if((millis()-timer2)>200) send_BT(); } voidsend_BT() { intt; intx; inty; if(roll>-100&&roll<100) x=map(roll,-100,100,,100); if(pitch>-100&&pitch<100) y=map(pitch,-100,100,100,200); if(temp>&&temp<50) t=200+int(temp); BT.write(x); BT.write(y); BT.write(t); timer2=millis(); } |
Погрешность гироскопа – «дрифт» (drift)
Из-зза неидеальной калибровки гироскопа, “gyro_x_scalled” никогда не равна нулю и со временем “angle_x_gyro” изменяет свои значения. Для решения данной проблемы, проводится расчет угла с помощью акселерометра и полученные значения сравнывиются с углом гироскопа. Так как модуль MPU6050 располагается горизонтально, ускорение по оси z равно 1g (то есть, 9.81) как это показано на рисунке. Мы можем использовать этот вектор ускорения и его проекцию на ось y для расчета угла между осями x и y.
Угол, который рассчитывается с помощью акселерометра, рассчитывается по зависимости:
Основными проблемами при определении угла наклона с помощью акселерометра являются: сильная зашумленность сигнала и очень сильная чувствительность к вибрациям, без которых ни один механизм не работает. Более того, еслипри перемещении MPU6050 вдоль одной из осей координат, полученные значения будут мешать расчету угла. Так что для лучшего результата, углы с гироскопа и акселерометра объединяются с помощью фильтра:
Окончательно уравнение для определения угла наклона принимает вид:
На рисунке ниже приведена имплементация полученных зависимостей в оболочке Arduino IDE
How It Works
The MPU6050 IMU has both 3-Axis accelerometer and 3-Axis gyroscope integrated on a single chip.
The gyroscope measures rotational velocity or rate of change of the angular position over time, along the X, Y and Z axis. It uses MEMS technology and the Coriolis Effect for measuring, but for more details on it you can check my particular How MEMS Sensors Work tutorial. The outputs of the gyroscope are in degrees per second, so in order to get the angular position we just need to integrate the angular velocity.
On the other hand, the MPU6050 accelerometer measures acceleration in the same way as explained in the previous video for the ADXL345 accelerometer sensor. Briefly, it can measure gravitational acceleration along the 3 axes and using some trigonometry math we can calculate the angle at which the sensor is positioned. So, if we fuse, or combine the accelerometer and gyroscope data we can get very accurate information about the sensor orientation.
The MPU6050 IMU is also called six-axis motion tracking device or 6 DoF (six Degrees of Freedom) device, because of its 6 outputs, or the 3 accelerometer outputs and the 3 gyroscope outputs.
See Also
Установка платы датчика Холла
Во-первых, для реализации тахометра, мне нужен был небольшой неодимовый магнит, который нужно было прикрепить на вал шпинделя. Перерыл все ящики — я ничего подходящего не нашел. Зато нашел старый, нерабочий cd-rom от ноутбука. Вот в нем, в катушке электромагнита открывания, как раз и нашел, то, что нужно — небольшой, прямоугольный неодимовый магнит!
Определив высоту и полярность, я приклеил магнит к валу на «суперклей» и обтянул вал с магнитом термоусадкой. На копус шпинделя приклеил прокладку, а уже на прокладку — плату. Как видите — получилось довольно аккуратно. Защитный колпачек в процессе обдумывания, так что, пока без него
Изготовление печатной платы для шагомера на основе ATtiny85
Для проектирования печатной платы мы использовали редактор EasyEDA. 3D модель спроектированной нами печатной платы для этого проекта выглядит следующим образом:
Заказ печатной платы с сервиса PCBWay
Для заказа печатной платы с сервиса PCBWay (разумеется, вы можете использовать любой удобный вам способ заказа печатной платы) выполните следующую последовательность шагов.
Шаг 1. Перейдите на сайт https://www.pcbway.com, зарегистрируйтесь на нем если вы заходите на него в первый раз. На вкладке PCB Prototype (прототип печатной платы) укажите размеры печатной платы, число ее слоев и необходимое число плат.
Шаг 2. Нажмите на кнопку ‘Quote Now’ (заказать сейчас). После этого вас перебросит на страницу, на которой вам необходимо будет ввести дополнительные параметры печатной платы: ее тип, слои, материал, толщину и т.д. Большинство этих параметров можно оставить такими, какими их сервис предлагает по умолчанию.
Шаг 3. В заключение вам необходимо загрузить в сервис Gerber файлы и оплатить заказ. Перед переходом к процессу оплаты сервис PCBWAY проверяет ваши Gerber файлы на корректность.
Сборка шагомера на печатной плате
Нашу плату сервис изготовил за несколько дней, ее качество было на высоте. Низ и верх нашей печатной платы показаны на следующем рисунке.
После припаивания компонентов проекта к печатной плате у нас получилась конструкция следующего вида:
Процесс работы MPU6050
MPU6050 IMU объединяет 3-осевой акселерометр и 3-осевой гироскоп на одном чипе.
Гироскоп измеряется вдоль момента измерения оси X, Y и Z, угловое положение с скоростью вращения или скоростью изменения. Измеряется с использованием технологии MEMS и эффекта Кориоли. Выход гироскопа находится в единицах в секунду, поэтому нам нужно только интегрировать диагональную скорость, чтобы получить угловое положение.
MPU6050 IMU 3 AXIS акселерометр и 3 оси гироскоп
С другой стороны, акселерометр MPU 6050 измерял такой же, как датчик ускорения ADXL345. Вкратце, он может измерить ускорение гравитации вдоль 3 валов и использовать немного треугольной школы математики, мы можем рассчитать угол позиционирования датчика. Следовательно, если мы предопределяем или комбинируем акселерометры и данные гиросков, мы можем получить очень точную информацию о направлении датчиков.
MPU6050 IMU также известен как устройства отслеживания шестисей движения или 6 устройств DOF (шесть степеней свободы), потому что у него есть шесть выходов, то есть 3 выхода измерителя ускорения и 3 выхода гироскопа.
Тестирование
После отладки необходимо провести тестирование. В чём разница? При тестировании вы точно знаете, что программный код работает без лагов и багов, но вам необходимо убедиться, что в нём нет логических ошибок.
С акселерометром и гироскопом проще всего использовать программы 3Д рендеринга показаний, вроде ShowGY521Data, которые позволят в реальном времени увидеть, как железо позиционируется в пространстве. В случае неисправностей всегда можно подправить нулевой уровень и уменьшить чувствительность акселерометра, который также влияет на конечную модель отображения устройства.
Комплектующие
Создаётся данный датчик или МК, в зависимости от того, что вы собрались приобретать, из компонентов ATmega328.
Распиновка модуля Arduino MPU 6050
Так, в нём имеются:
- 14 штук различных пинов и цифровых выходов, половина из которых являются ШИМ-выходами.
- Специальные кварцевые резонаторы до 16 МГц мощностью.
- Встроенный вход под usb-кабель, который позволит вам сэкономить не только время, но и деньги, которые вы могли бы потратить на покупку адаптера.
- Контакты и распиновка для стандартного питания с нулем, фазой и заземлением.
- Контакты для сброса до заводских настроек, при которых весь машинный код и данные будут стёрты. Это полезно в том случае, если вы напортачили с программой и модуль превратился в бесполезную груду железа, и просто как экономия времени, если необходимо сменить прошивку.
- ICSP контакт, который необходим для того, чтобы запрограммировать машинный код, который будет находиться внутри системы.
Все эти компоненты и составляют Arduino гироскоп, позволяя ему выполнять свои базовые функции. Но как же запрограммировать систему, если вы до этого не имели опыта работа с данными МК?
Шаг 3. Программируем Arduino для обработки информации, полученной с акселерометра
Алгоритм написания программы в последовательности:
// Подключаем необходимые для работы библиотек #include "MPU6050.h"; #include "I2Cdev.h"; #include "Wire.h"; // Создаем объект, символизирующий модуль датчика MPU6050 axeler; // Создаем объект библиотеки Wire Wire b; // Создаем объект, который символизирует контакт I2C I2Cdev h; // Вводим цифровые данные, отвечающие за точки в 3-х осях int16_t axx, axy, axz; int16_t gix, giy, giz; // Объявляем метод, который будет запускать программу void setup() { // Начинаем работу Wire.begin(); h.begin(38400); // Производим инициализацию, отчет выводится после компиляции h.println("Initializing I2C devices..."); axeler.initialize(); delay(100); } // Считываем значения гироскопа и акселерометра с помощью адресов, которые принадлежат описанным выше переменным void loop() { axeler.getMotion6(&axx, &axy, &axz, &gix, &giy, &giz); // Выводим получившиеся значения на экран h.print("a/g:t"); h.print(axx); h.print("t"); h.print(axy); hl.print("t"); hl.print(axz); h.print("t"); h.print(gix); h.print("t"); h.print(giy); h.print("t"); h.println(giz); }
Вуаля! Акселерометр Аrduino запрограммирован.
В принципе, для разнообразия можно написать еще один скетч (ниже), но тогда нам нужна будет еще одна библиотека — Kalman (Gy-521, mpu6050), которая преобразует показания координат X и Y.
#include #include "Kalman.h" Kalman kalmanX; Kalman kalmanY; uint8_t IMUAddress = 0x68; /* IMU Data */ int16_t accX; int16_t accY; int16_t accZ; int16_t tempRaw; int16_t gyroX; int16_t gyroY; int16_t gyroZ; double accXangle; // Angle calculate using the accelerometer double accYangle; double temp; double gyroXangle = 180; // Angle calculate using the gyro double gyroYangle = 180; double compAngleX = 180; // Calculate the angle using a Kalman filter double compAngleY = 180; double kalAngleX; // Calculate the angle using a Kalman filter double kalAngleY; uint32_t timer; void setup() { Wire.begin(); Serial.begin(9600); i2cWrite(0x6B,0x00); // Disable sleep mode kalmanX.setAngle(180); // Set starting angle kalmanY.setAngle(180); timer = micros(); } void loop() { /* Update all the values */ uint8_t* data = i2cRead(0x3B,14); accX = ((data << 8) | data); accY = ((data << 8) | data); accZ = ((data << 8) | data); tempRaw = ((data << 8) | data); gyroX = ((data << 8) | data); gyroY = ((data << 8) | data); gyroZ = ((data << 8) | data); /* Calculate the angls based on the different sensors and algorithm */ accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG; accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG; double gyroXrate = (double)gyroX/131.0; double gyroYrate = -((double)gyroY/131.0); gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000); kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000); timer = micros(); Serial.println(); Serial.print("X:"); Serial.print(kalAngleX,0); Serial.print(" "); Serial.print("Y:"); Serial.print(kalAngleY,0); Serial.println(" "); // The accelerometer's maximum samples rate is 1kHz } void i2cWrite(uint8_t registerAddress, uint8_t data){ Wire.beginTransmission(IMUAddress); Wire.write(registerAddress); Wire.write(data); Wire.endTransmission(); // Send stop } uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) { uint8_t data; Wire.beginTransmission(IMUAddress); Wire.write(registerAddress); Wire.endTransmission(false); // Don't release the bus Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading for(uint8_t i = 0; i < nbytes; i++) data = Wire.read(); return data; }
После второго скетча вы на экране сможете увидеть подобные цифры:
Шаг 3. Программируем Arduino для обработки информации, полученной с акселерометра
Алгоритм написания программы в последовательности:
// Подключаем необходимые для работы библиотек
#include "MPU6050.h";
#include "I2Cdev.h";
#include "Wire.h";
// Создаем объект, символизирующий модуль датчика
MPU6050 axeler;
// Создаем объект библиотеки Wire
Wire b;
// Создаем объект, который символизирует контакт I2C
I2Cdev h;
// Вводим цифровые данные, отвечающие за точки в 3-х осях
int16_t axx, axy, axz;
int16_t gix, giy, giz;
// Объявляем метод, который будет запускать программу
void setup()
{
// Начинаем работу
Wire.begin();
h.begin(38400);
// Производим инициализацию, отчет выводится после компиляции
h.println("Initializing I2C devices...");
axeler.initialize();
delay(100);
}
// Считываем значения гироскопа и акселерометра с помощью адресов, которые принадлежат описанным выше переменным
void loop()
{
axeler.getMotion6(&axx, &axy, &axz, &gix, &giy, &giz);
// Выводим получившиеся значения на экран
h.print("a/g:\t");
h.print(axx);
h.print("\t");
h.print(axy);
hl.print("\t");
hl.print(axz);
h.print("\t");
h.print(gix);
h.print("\t");
h.print(giy);
h.print("\t");
h.println(giz);
}
Вуаля! Акселерометр Аrduino запрограммирован.
В принципе, для разнообразия можно написать еще один скетч (ниже), но тогда нам нужна будет еще одна библиотека — Kalman (Gy-521, mpu6050), которая преобразует показания координат X и Y.
#include <Wire.h>
#include "Kalman.h"
Kalman kalmanX;
Kalman kalmanY;
uint8_t IMUAddress = 0x68;
/* IMU Data */
int16_t accX;
int16_t accY;
int16_t accZ;
int16_t tempRaw;
int16_t gyroX;
int16_t gyroY;
int16_t gyroZ;
double accXangle; // Angle calculate using the accelerometer
double accYangle;
double temp;
double gyroXangle = 180; // Angle calculate using the gyro
double gyroYangle = 180;
double compAngleX = 180; // Calculate the angle using a Kalman filter
double compAngleY = 180;
double kalAngleX; // Calculate the angle using a Kalman filter
double kalAngleY;
uint32_t timer;
void setup() {
Wire.begin();
Serial.begin(9600);
i2cWrite(0x6B,0x00); // Disable sleep mode
kalmanX.setAngle(180); // Set starting angle
kalmanY.setAngle(180);
timer = micros();
}
void loop() {
/* Update all the values */
uint8_t* data = i2cRead(0x3B,14);
accX = ((data << 8) | data);
accY = ((data << 8) | data);
accZ = ((data << 8) | data);
tempRaw = ((data << 8) | data);
gyroX = ((data << 8) | data);
gyroY = ((data << 8) | data);
gyroZ = ((data << 8) | data);
/* Calculate the angls based on the different sensors and algorithm */
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gyroX/131.0;
double gyroYrate = -((double)gyroY/131.0);
gyroXangle += kalmanX.getRate()*((double)(micros()-timer)/1000000); // Calculate gyro angle using the unbiased rate
gyroYangle += kalmanY.getRate()*((double)(micros()-timer)/1000000);
kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()-timer)/1000000); // Calculate the angle using a Kalman filter
kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()-timer)/1000000);
timer = micros();
Serial.println();
Serial.print("X:");
Serial.print(kalAngleX,0);
Serial.print(" ");
Serial.print("Y:");
Serial.print(kalAngleY,0);
Serial.println(" ");
// The accelerometer's maximum samples rate is 1kHz
}
void i2cWrite(uint8_t registerAddress, uint8_t data){
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.write(data);
Wire.endTransmission(); // Send stop
}
uint8_t* i2cRead(uint8_t registerAddress, uint8_t nbytes) {
uint8_t data;
Wire.beginTransmission(IMUAddress);
Wire.write(registerAddress);
Wire.endTransmission(false); // Don't release the bus
Wire.requestFrom(IMUAddress, nbytes); // Send a repeated start and then release the bus after reading
for(uint8_t i = 0; i < nbytes; i++)
data = Wire.read();
return data;
}
После второго скетча вы на экране сможете увидеть подобные цифры:
