MPU6050 akcelerometer a gyroskop pre Arduino

V minulej lekcii, Arduino a práca s tlačidlami - Knižnica , sme vytvorili OOP knižnicu pre prácu s tlačidlom pre pohodlné znovu-používaní kódu v Arduino projektoch.

Modul MPU6050 IMU pre Arduino sa skladá z akcelerometra a gyroskopu, vďaka ktorým môžeme merať zrýchlenie a uhlovú rýchlosť v 3 osách. Celkovo máme k dispozícii 6 hodnôt (6 stupňov voľnosti), preto sa tento modul označuje ako 6 DOF (angl. Six Degrees of Freedom). Z nameraných hodnôt vieme určiť uhly pootočenie (v angličtine sa označujú ako yaw, roll a pitch). Modul je tak vhodný napr. Pre určenie orientácie Drone alebo robotické ruky.

Princíp merania

Obaja senzory, akcelerometer a gyroskop, patrí do tzv. MEMS (Mikro-Elektro-Mechanické Systémy). Jedná sa o konštrukcia veľmi malých rozmerov (od 0.001 mm po 0.1 mm). K výrobe sa využíva technológiou z produkcie mikroelektroniky, ako je napr. Selektívne leptanie alebo iónové odprašovanie.

Akcelerometer

Zrýchlenie získavame meraním kapacity medzi pohyblivým telom a fixnými platňami. Pri zrýchlení telesa dochádza k zmene kapacity medzi doskami, ktorú následne softvérovo prepočítame na zrýchlenie v jednotkách m / s ² alebo v jednotkách preťaženia (násobky zemskej tiaži v jednotkách g).

Zdroj obrázku: howtomechatro­nics.com

Gyroskop

Gyroskop meria uhlovú rýchlosť (zmena uhla pootočenie za určitý čas) využitím Coriolisovej sily. Pri pohybe telesa v smere vektora rýchlosti (viď obrázok nižšie) a vystavenie telesa vonkajšiemu otáčania (uhlová rýchlosť) dochádza k tomu, že na teleso pôsobí Coriolisova sila. Táto sila spôsobí premiestnenie telesa z jeho pôvodnej polohy niekam inam (angl. Displacement). Táto zmena polohy spôsobí zmenu v kapacite, ktorá sa dá zmerať a spracovať podobne ako v prípade akcelerometra.

Zdroj obrázku: howtomechatro­nics.com

Zapojenie modulu

Modul komunikuje prostredníctvom Aj ^ 2C protokolu, takže zapojenie nie je problém, viď obrázok. Napájať senzor môžeme s 3.3 V alebo 5 V:

Zdroj obrázku: howtomechatro­nics.com

Kód

Tentoraz si kód sťažíme tým, že nebudeme využívať externé knižnicu od Adafruitu. Tieto senzory možno totiž kúpiť aj lacnejšie neoriginálne a už sa mi niekoľkokrát stalo, že klony potom s Adafruit knižnicou nefungovali. Pre komunikáciu so senzorom budeme potrebovať iba knižnicu Wire.h. Takže sa tešte na prácu s registrami.

Najprv knižnicu naincludujeme:

#include <Wire.h>

Na začiatok si deklarujeme všetky premenné:

• 6 stupňov voľnosti (zrýchlenie a uhlová rýchlosť v osiach x, y, z)
• 2 uhly získame Trigonometrické metódou z akcelerometra
• 3 uhly z gyroskopu pre určenie orientácie senzora
• ďalej si budeme ukladať celkové zrýchlenie a celkové preťaženie G v násobkoch zemskej tiaže
• ďalej namerané odchýlky v pokoji, ktoré slúžia na spresnenie meranie (chybové hodnoty)
• a nakoniec premenné na určenie času

V kóde to všetko bude vyzerať nasledovne:

const int MPU = 0x68;                                             // MPU6050 I2C adresa
float AccX, AccY, AccZ;                                           // zrychlení ve směru osy x, y, z
float GyroX, GyroY, GyroZ;                                        // úhlová rychlost ve směru osy x, y, z
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;   // úhly pootočení ve směrech x, y, z
float roll, pitch, yaw;                                           // názvy proměnných, které se běžně používají v letecké terminologii
float a;                                  // celkové zrychlení v m/s^2
float G;                                  // přetížení v násobcích zemské tíhy (1g = 9.81 m/s^2)
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;   // chybové hodnoty
int c = 0;                                                        // proměnná pro cyklus while

// proměnné pro určení času, za který se senzor pootočil o daný úhel
float elapsedTime, currentTime, previousTime;

setup()

V setup() najprv resetujeme senzor. Komunikáciu začneme pomocou príkazov Wire.begin() a Wire.beginTransmission("adresa senzoru"). Adresu senzora sme na začiatku nastavili na 0x68. Následne, aby sme resetovali senzor, musíme do registra s adresou 6B do 7. bitu zadať 0. Do ostatných bitov zapíšeme tiež 0, pretože nič iné, než reset nepotrebujeme:

Serial.begin(9600);

Wire.begin();           // inicializace I2C komunikace
Wire.beginTransmission (MPU);   // inicializace komunikace přímo se senzorem MPU6050 // MPU = 0x68
Wire.write(0x6B);       // komunikace s registrem 6B
Wire.write(0x00);       // reset senzoru - umístění nul do registru 6B
Wire.endTransmission(true);     // konec přenosu

Treba dodať, že Aj ^ 2C protokol prenáša dáta so sekvenciou 8 bitov (8 núl / jedničiek). Do príkazu Wire.write("sekvence bitů") môžeme zadať sekvenciu buď v hexadecimálnej alebo binárnej sústave. Takže Wire.write(00000000) je ekvivalentná Wire.write(0x00). My budeme ďalej pracovať v hexadecimálnej sústave. Nakoniec ukončíme prenos s príkazom Wire.endTransmission(true).

Ďalej nastavíme rozsah akcelerometra tak, že pristúpime k registra 1C a zmeníme 4. a 5. bit. Rozsahy možné meniť od +/- 2G až po +/- 16 G. V kóde stačí odkomentovať príslušný riadok, aktuálna je tu ponechané +/- 2G.

// Nastavení rozsahu akcelerometru

Wire.beginTransmission (MPU);
Wire.write (0x1C); // přístup k ACCEL_CONFIG registru (1C v hex), nastav rozsah, odkomentuj řádek
Wire.write(0x00); // +/- 2G AFS_SEL = 0, v binární soustavě 00000000
//Wire.write(0x08); // +/- 4G AFS_SEL = 1, v binární soustavě 00001000
//Wire.write(0x10); // +/- 8G AFS_SEL = 2, v binární soustavě 00010000
//Wire.write(0x18); // +/- 16G AFS_SEL = 3, v binární soustavě 00011000
Wire.endTransmission (true);

Nastavíme rozsah gyroskopu. Gyroskop meria uhlovú rýchlosť, v podstate sa dá zmerať uhlová rýchlosť približne 5 otáčok za sekundu (360 ° / s je 1 otáčka za sekundu). K nastaveniu slúžia register 1B, princíp je rovnaký ako u akcelerometra:

// Nastavení rozsahu gyroskopu

Wire.beginTransmission(MPU);
Wire.write (0x1B); // komunikace s registrem 1B
//Wire.write(0x00); // +/- 250 ° / s AFS_SEL = 0, v binární soustavě 000 00 000
//Wire.write(0x08); // +/- 500 ° / s AFS_SEL = 1, v binární soustavě 000 01 000
//Wire.write(0x10); // +/- 1000 ° / s AFS_SEL = 2, v binární soustavě 000 10 000
//Wire.write(0x18); // +/- 2000 ° / s AFS_SEL = 3, v binární soustavě 000 11 000
Wire.endTransmission(true);
delay(20);

Žiadny senzor nie je 100%. Preto je vhodné na začiatku zistiť odchýlku od skutočnej hodnoty, ktorú by sme chceli namerať. Stačí senzor nechať v pokoji a funkcie nám vráti odchýlky, ktoré je potrebné odpočítať od nameraných hodnôt. V podstate ide o cyklus, v ktorom 200 krát zistíme raw hodnoty z akcelerometra a gyroskopu, a následne hodnoty spriemerujeme. Telo funkcie si môžete pozrieť v závere článku.

// zavolání funkce calculate_IMU_error() pro získání odchylky (chybové hodnoty)
calculate_IMU_error();

loop()

Raw dáta z akcelerometra získame tak, že si vyžiadame 6 registrov (3B, 3C, 3D, 3E, 3F, 40) príkazom Wire.requestFrom(MPU, 6, true). Predtým si však zapíšeme prvý register, 3B. Získame tak sekvenciu 48 bitov (6 registrov po 8 bitov). Príkazom Wire.read() získame jednotlivé bity, posun vykonávame operátorom <<:

// data z akcelerometru
Wire.beginTransmission(MPU);
Wire.write(0x3B); // začneme s registrem 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // vyžádání 6 registrů, každá hodnota pro 1 osu je uložena v 2 registrech

/* Pro rozsah + -2G vyděl raw hodnoty 8192
 * Pro rozsah + -4G vyděl raw hodnoty 4096
 * Pro rozsah + -8G vyděl raw hodnoty 2048
 * Pro rozsah + -16G vyděl raw hodnoty 1024
 */
// pro každou hodnotu zjistíme hodnoty v 2 registrech, sečteme (symbol | = the bitwise OR operátor)

AccX = (Wire.read () << 8 | Wire.read ()) / 8192.0; // X osa
AccY = (Wire.read () << 8 | Wire.read ()) / 8192.0; // Y osa
AccZ = (Wire.read () << 8 | Wire.read ()) / 8192.0; // Z osa

Raw hodnotu napr. Pre os x získame sčítaním bitov z registra 3B a 3C. Každá hodnota je ukrytá v 2 registroch. Nejde priamo o sčítaní, ale o operácii OR v binárnej sústave (symbol |). Získanú hodnotu vydelíme číslom 8192 (ak sme si zvolili rozsah 2G), ktoré zistíme z datasheete. Pre iných rozsahy delíme iným číslom, pozri kód.

Z nameraných hodnôt zrýchlenia môžeme určiť uhly pootočenie pre os x a y. Postačí na to funkcia atan(), ktorá zistí uhol z pomeru jednotlivých hodnôt:

// určení úhlu x a y trigonometrickou metodou a ošetření hodnot odečtením odchylky
accAngleX = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~ (+0.58)
accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~ (-1.58)

U gyroskopu je to podobné, líši sa však výpočet uhlov. Zo senzoru získame uhlovú rýchlosť, ktorú treba vynásobiť časom, za ktorý sa senzor pootočil (podobne ako dráha = rýchlosť * čas). Ten určíme ako rozdiel aktuálneho času a čas z predchádzajúceho cyklu.

// data z gyroskopu
previousTime = currentTime;                 // získání času z předchozí smyčky
currentTime = Millis ();                // aktuální čas
elapsedTime = (currentTime - previousTime) / 1000;  // vypočítá čas, za který se senzor pootočil o určitý úhel

Ukážme si kód pre gyroskop, princíp je rovnaký ako u akcelerometra. Začneme registrom 0x43 a zmeníme deliteľa pre raw hodnotu podľa rozsahu gyroskopu:

Wire.beginTransmission(MPU);
Wire.write(0x43); // začneme s registrem 0x43
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true); // podobně jako u akcelerometru

/* Pro rozsah 250deg / s vyděl raw hodnoty 131
 * Pro rozsah 250deg / s vyděl raw hodnoty 65.5
 * Pro rozsah 250deg / s vyděl raw hodnoty 32.8
 * Pro rozsah 250deg / s vyděl raw hodnoty 16.4
 */

GyroX = (Wire.read() << 8 | Wire.read()) / 131.0;
Gyro = (Wire.read() << 8 | Wire.read()) / 131.0;
GyroZ = (Wire.read() << 8 | Wire.read()) / 131.0;

// ošetření hodnot odečtením odchylky, pro každý senzor jsou hodnoty jiné!
GyroX = GyroX +0.16; // GyroErrorX ~ (-0.56)
Gyro = gyro -4.22; // GyroErrorY ~ (2)
GyroZ = GyroZ -0.26; // GyroErrorZ ~ (-0.8)

Nasleduje vyčíslenie uhlov. Treba dodať, že k aktuálnemu uhla pričítame uhol z predchádzajúceho cyklu. Môžeme tak namerať aj pootočenie o 1000 ° (rozsah je pre uhlovú rýchlosť, nie pre uhol!).

// úhly získáme tak, že naměřené hodnoty úhlové rychlosti vynásobíme časovým intervalem
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // stupeň / sekunda * sekunda = stupeň
gyroAngleY = gyroAngleY + gyro * elapsedTime;
yaw = yaw + GyroZ * elapsedTime; // namísto yaw jsme mohli dát gyroAngleZ

// pro získání přesnějších měření kombinujeme, 96% hodnoty bude tvořit hodnota z gyroskopu, 4% z akcelerometru
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

Yaw, roll a pitch sú jednoducho názvy uhlov, ktoré sú zaužívané v leteckej terminológii. V kóde by sa dal napríklad. Roll vypočítať takto: roll = gyroAngleX + GyroX * elapsedTime. V kóde sme vzali 96% z gyroskopu a 4% z akcelerometra pre určenie uhlov roll a pitch, preto to vyzerá trošku inak.

Zdroj obrázku: howtomechatro­nics.com

Celkové zrýchlenie a celkové preťaženie vypočítame ako vektorový súčet zrýchlenie takto:

// celkové přetížení,
G = sqrt (AccX * AccX + AccY * AccY + AccZ * AccZ) -1.03;

// celkové zrychlení
a = 9.81 * G;

Možno sa pýtate, prečo je tam to číslíčko 1.03 ? Ak je modul v pokoji, senzor v smere osi xayv ideálnom prípade nameria zrýchlenie 0. V smere osi z pôsobí však gravitačné zrýchlenie aj keď je modul v pokoji (spomeňte si na pohyblivé telo na pružinách). AccZ je približne 1 g. Experimentálne som zistil, že mi to zhruba vychádza na 1.03 pre celkové zrýchlenie, viď obrázok:

Zdroj obrázku: howtomechatro­nics.com

Záver

Senzor reaguje naozaj s dobrou odozvou a citlivosť je vysoká. Senzor som položil na stôl a dokáže detekovať napr. Písanie na klávesnici alebo búchanie do stola:

Celý kód

#include <Wire.h>
const int MPU = 0x68;                                             // MPU6050 I2C adresa
float AccX, AccY, AccZ;                                           // zrychlení ve směru osy x, y, z
float GyroX, GyroY, GyroZ;                                        // uhlová rychlost ve směru osy x, y, z
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;   // úhly pootočení ve směrech x, y, z
float roll, pitch, yaw;                                           // názvy proměnných, které se běžně používají v letecké terminologii
float a;                    // celkové zrychlení v m/s^2
float G;                    // přetížení v násobcích zemské tíže (1g = 9.81 m/s^2)
float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ;   // chybové hodnoty
int c = 0;                                                        // proměnná pro cyklus while

// proměnné pro určení času, za který se senzor pootočil o daný úhel
float elapsedTime, currentTime, previousTime;

void setup() {
Serial.begin(9600);
Wire.begin ();      // inicializace I2C komunikace
Wire.beginTransmission (MPU);   // inicializace komunikace přímo se senzorem MPU6050 // MPU = 0x68
Wire.write (0x6B);    // komunikace s registrem 6B
Wire.write (0x00);    // reset senzoru - umístění nul do registru 6B
Wire.endTransmission (true);  // konec přenosu

// Nastavení rozsahu akcelerometru

Wire.beginTransmission (MPU);
Wire.write (0x1C); // přístup k ACCEL_CONFIG registru (1C v hex), nastav rozsah, odkomentuj řádek
//Wire.write(0x00); // +/- 2G AFS_SEL = 0, v binární soustavě 00000000
//Wire.write(0x08); // +/- 4G AFS_SEL = 1, v binární soustavě 00001000
//Wire.write(0x10); // +/- 8G AFS_SEL = 2, v binární soustavě 00010000
//Wire.write(0x18); // +/- 16G AFS_SEL = 3, v binární soustavě 00011000
Wire.endTransmission (true);

// Nastavení rozsahu gyroskopu

Wire.beginTransmission (MPU);
Wire.write (0x1B); // komunikace s registrem 1B
//Wire.write(0x00); // +/- 250 ° / s AFS_SEL = 0, v binární soustavě 000 00 000
//Wire.write(0x08); // +/- 500 ° / s AFS_SEL = 1, v binární soustavě 000 01 000
//Wire.write(0x10); // +/- 1000 ° / s AFS_SEL = 2, v binární soustavě 000 10 000
//Wire.write(0x18); // +/- 2000 ° / s AFS_SEL = 3, v binární soustavě 000 11 000
Wire.endTransmission (true);
delay (20);


// zavolání funkce calculate_IMU_error () pro získání odchylky (chybové hodnoty)
calculate_IMU_error();
delay(20);
}

void loop() {

// data z akcelerometru
Wire.beginTransmission (MPU);
Wire.write (0x3B); // začneme s registrem 0x3B (ACCEL_XOUT_H)
Wire.endTransmission (false);
Wire.requestFrom (MPU, 6, true); // vyžádání 6 registrů, každá hodnota pro 1 osu je uložena v 2 registrech

/* Pro rozsah + -2G vyděl raw hodnoty 8192
 * Pro rozsah + -4G vyděl raw hodnoty 4096
 * Pro rozsah + -8G vyděl raw hodnoty 2048
 * Pro rozsah + -16G vyděl raw hodnoty 1024
 */
// pro každou hodnotu zjistíme hodnoty v 2 registrech, sečteme (symbol | = the bitwise OR operátor)

AccX = (Wire.read () << 8 | Wire.read ()) / 8192.0; // X os
AccY = (Wire.read () << 8 | Wire.read ()) / 8192.0; // Y os
AccZ = (Wire.read () << 8 | Wire.read ()) / 8192.0; // Z os

// určení úhlu x a y trigonometrickou metodou a ošetření hodnot odečtením odchylky
accAngleX = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~ (+0.58)
accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~ (-1.58)

// data z gyroskopu

Wire.beginTransmission (MPU);
Wire.write (0x43); // začneme s registrem 0x43
Wire.endTransmission (false);
Wire.requestFrom (MPU, 6, true); // podobně jako u akcelerometru

/* Pro rozsah 250deg / s vyděl raw hodnoty 131
 * Pro rozsah 250deg / s vyděl raw hodnoty 65.5
 * Pro rozsah 250deg / s vyděl raw hodnoty 32.8
 * Pro rozsah 250deg / s vyděl raw hodnoty 16.4
 */

GyroX = (Wire.read () << 8 | Wire.read ()) / 131.0;
GyroY = (Wire.read () << 8 | Wire.read ()) / 131.0;
GyroZ = (Wire.read () << 8 | Wire.read ()) / 131.0;

// ošetření hodnot odečtením odchylky, pro každý senzor jsou hodnoty jiné!
GyroX = GyroX +0.16; // GyroErrorX ~ (-0.56)
GyroY = GyroY -4.22; // GyroErrorY ~ (2)
GyroZ = GyroZ -0.26; // GyroErrorZ ~ (-0.8)

// úhly získáme tak, že naměřené hodnoty úhlové rychlosti vynásobíme časovým intervalem
gyroAngleX = gyroAngleX + GyroX * elapsedTime; // stupeň / sekunda * sekunda = stupeň
gyroAngleY = gyroAngleY + GyroY * elapsedTime;
yaw = yaw + GyroZ * elapsedTime; // namísto yaw jsme mohli dát gyroAngleZ

// pro získání přesnějších měření kombinujeme, 96% hodnoty bude tvořit hodnota z gyroskopu, 4% z akcelerometru
roll = 0.96 * gyroAngleX + 0.04 * accAngleX;
pitch = 0.96 * gyroAngleY + 0.04 * accAngleY;

// celkové přetížení,
G = sqrt (AccX * AccX + AccY * AccY + AccZ * AccZ) -1.03;

// celkové zrychlení
a = 9.81 * G;

// vypsání hodnot
Serial.print(" zrychlení ");
Serial.print(" osa x ");
Serial.print(AccX);
Serial.print(" osa y ");
Serial.print(AccY);
Serial.print(" osa z ");
Serial.println(AccZ);

Serial.print(" úhly ");
Serial.print(" osa x ");
Serial.print(gyroAngleX);
Serial.print(" osa y ");
Serial.print(gyroAngleY);
Serial.print(" osa z ");
Serial.println(yaw);

Serial.print(" přetížení ");
Serial.println(G);
Serial.print(" celkové zrychlení ");
Serial.println(a);

Serial.println(); // mezera

}

void calculate_IMU_error() {
// odchylku zobrazíme v serial monitoru, a následně přepíšeme do kódu
// cyklus while, průměr 200 krát
while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);

// nezapomenout při změně rozsahu měnit dělitele
AccX = (Wire.read() << 8 | Wire.read()) / 8192.0 ;
AccY = (Wire.read() << 8 | Wire.read()) / 8192.0 ;
AccZ = (Wire.read() << 8 | Wire.read()) / 8192.0 ;

// sčítání naměřených hodnot
AccErrorX = AccErrorX + ((atan((AccY) / sqrt(pow((AccX), 2) + pow((AccZ), 2))) * 180 / PI));
AccErrorY = AccErrorY + ((atan(-1 * (AccX) / sqrt(pow((AccY), 2) + pow((AccZ), 2))) * 180 / PI));
c++;
}

// vydělení naměřených hodnot číslem 200, získání průměrné odchylky
AccErrorX = AccErrorX / 200;
AccErrorY = AccErrorY / 200;
c = 0;

// totéž pro gyroskop

while (c < 200) {
Wire.beginTransmission(MPU);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(MPU, 6, true);
GyroX = Wire.read() << 8 | Wire.read();
GyroY = Wire.read() << 8 | Wire.read();
GyroZ = Wire.read() << 8 | Wire.read();
GyroErrorX = GyroErrorX + (GyroX / 131.0);
GyroErrorY = GyroErrorY + (GyroY / 131.0);
GyroErrorZ = GyroErrorZ + (GyroZ / 131.0);
c++;
}

GyroErrorX = GyroErrorX / 200;
GyroErrorY = GyroErrorY / 200;
GyroErrorZ = GyroErrorZ / 200;

// Vytiskněme chybové hodnoty na sériovém monitoru
Serial.print("AccErrorX: ");
Serial.println(AccErrorX);
Serial.print("AccErrorY: ");
Serial.println(AccErrorY);
Serial.print("GyroErrorX: ");
Serial.println(GyroErrorX);
Serial.print("GyroErrorY: ");
Serial.println(GyroErrorY);
Serial.print("GyroErrorZ: ");
Serial.println(GyroErrorZ);
}