Lekce 22 - MPU6050 akcelerometr a gyroskop pro Arduino

V minulé lekci, Arduino - Elektronická hrací kostka, jsme se naučili na LCD displeji simulovat hrací kostku.

Modul MPU6050 IMU pro Arduino se skládá z akcelerometru a gyroskopu, díky kterým můžeme měřit zrychlení a úhlovou rychlost ve 3 osách. Celkově máme k dispozici 6 hodnot (6 stupňů volnosti), proto se tento modul označuje jako 6 DOF (angl. Six Degrees of Freedom). Z naměřených hodnot umíme určit úhly pootočení (v angličtině se označují jako yaw, roll a pitch). Modul je tak vhodný např. pro určení orientace dronu nebo robotické ruky.

Princip měření

Oba senzory, akcelerometr a gyroskop, patří do tzv. MEMS (Mikro-Elektro-Mechanické Systémy). Jedná se o konstrukce velmi malých rozměrů (od 0.001 mm po 0.1 mm). K výrobě se využívá technologií z produkce mikroelektroniky, jako je např. selektivní leptání nebo iontové odprašování.

Akcelerometr

Zrychlení získáváme měřením kapacity mezi pohyblivým tělem a fixními plotnami. Při zrychlení tělesa dochází ke změně kapacity mezi deskami, kterou následně softwarově přepočítáme na zrychlení v jednotkách m/s² nebo v jednotkách přetížení (násobky zemské tíze v jednotkách g).

Zdroj obrázku: howtomechatro­nics.com

Gyroskop

Gyroskop měří úhlovou rychlost (změna úhlu pootočení za určitý čas) využitím Coriolisovy síly. Při pohybu tělesa ve směru vektoru rychlosti (viz obrázek níže) a vystavení tělesa vnějšímu otáčení (úhlová rychlost) dochází k tomu, že na těleso působí Coriolisova síla. Tato síla způsobí přemístění tělesa z jeho původní polohy někam jinam (angl. displacement). Tato změna polohy způsobí změnu v kapacitě, která se dá změřit a zpracovat podobně jako v případě akcelerometru.

Zdroj obrázku: howtomechatro­nics.com

Zapojení modulu

Modul komunikuje prostřednictvím I^2C protokolu, takže zapojení není problém, viz obrázek. Napájet senzor můžeme s 3.3 V nebo 5 V:

Zdroj obrázku: howtomechatro­nics.com

Kód

Tentokrát si kód ztížíme tím, že nebudeme využívat externí knihovnu od Adafruitu. Tyto senzory lze totiž koupit i levněji neoriginální a již se mi několikrát stalo, že klony pak s Adafruit knihovnou nefungovaly. Pro komunikaci se senzorem budeme potřebovat pouze knihovnu Wire.h. Takže se těšte na práci s registry.

Nejprve knihovnu naincludujeme:

#include <Wire.h>

Na začátek si deklarujeme všechny proměnné:

• 6 stupňů volnosti (zrychlení a úhlová rychlost v osách x, y, z)
• 2 úhly získáme trigonometrickou metodou z akcelerometru
• 3 úhly z gyroskopu pro určení orientace senzoru
• dále si budeme ukládat celkové zrychlení a celkové přetížení G v násobcích zemské tíhy
• dále naměřené odchylky v klidu, které slouží k zpřesnění měření (chybové hodnoty)
• a nakonec proměnné k určení času

V kódu to vše bude vypadat následovně:

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() nejprve resetujeme senzor. Komunikaci začneme pomocí příkazů Wire.begin() a Wire.beginTransmission("adresa senzoru"). Adresu senzoru jsme na začátku nastavili na 0x68. Následně, abychom resetovali senzor, musíme do registru s adresou 6B do 7. bitu zadat 0. Do ostatních bitů zapíšeme také 0, protože nic jiného, než reset nepotřebujeme:

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

Je třeba dodat, že I^2C protokol přenáší data se sekvencí 8 bitů (8 nul/jedniček). Do příkazu Wire.write("sekvence bitů") můžeme zadat sekvenci buď v hexadecimální nebo binární soustavě. Takže Wire.write(00000000) je ekvivalentní Wire.write(0x00). My budeme dále pracovat v hexadecimální soustavě. Nakonec ukončíme přenos s příkazem Wire.endTransmission(true).

Dále nastavíme rozsah akcelerometru tak, že přistoupíme k registru 1C a změníme 4. a 5. bit. Rozsahy lze měnit od +/- 2G až po +/- 16 G. V kódu stačí odkomentovat příslušný řádek, aktuálně je zde ponecháno +/- 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 měří úhlovou rychlost, v podstatě se dá změřit úhlová rychlost přibližně 5 otáček za sekundu (360 °/s je 1 otáčka za sekundu). K nastavení slouží registr 1B, princip je stejný jako u akcelerometru:

// 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);

Žádný senzor není 100%. Proto je vhodné na začátku zjistit odchylku od skutečné hodnoty, kterou bychom chtěli naměřit. Stačí senzor nechat v klidu a funkce nám vrátí odchylky, které je třeba odečíst od naměřených hodnot. V podstatě jde o cyklus, v němž 200 krát zjistíme raw hodnoty z akcelerometru a gyroskopu, a následně hodnoty zprůměrujeme. Tělo funkce si můžete prohlédnout v závěru článku.

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

loop()

Raw data z akcelerometru získáme tak, že si vyžádáme 6 registrů (3B, 3C, 3D, 3E, 3F, 40) příkazem Wire.requestFrom(MPU, 6, true). Předtím si však zapíšeme první registr, 3B. Získáme tak sekvenci 48 bitů (6 registrů po 8 bitů). Příkazem Wire.read() získáme jednotlivé bity, posun provádíme operátorem <<:

// 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 např. pro osu x získáme sečtením bitů z registru 3B a 3C. Každá hodnota je ukryta v 2 registrech. Nejde přímo o sčítání, ale o operaci OR v binární soustavě (symbol |). Získanou hodnotu vydělíme číslem 8192 (pokud jsme si zvolili rozsah 2G), které zjistíme z datasheetu. Pro jiné rozsahy dělíme jiným číslem, viz kód.

Z naměřených hodnot zrychlení můžeme určit úhly pootočení pro osu x a y. Postačí na to funkce atan(), která zjistí úhel z poměru jednotlivých hodnot:

// 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é, liší se však výpočet úhlů. Ze senzoru získáme úhlovou rychlost, kterou je třeba vynásobit časem, za který se senzor pootočil (podobně jako dráha = rychlost * čas). Ten určíme jako rozdíl aktuálního času a čas z předchozího 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

Ukažme si kód pro gyroskop, princip je stejný jako u akcelerometru. Začneme registrem 0x43 a změníme dělitele pro raw hodnotu dle 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)

Následuje vyčíslení úhlů. Je třeba dodat, že k aktuálnímu úhlu přičítáme úhel z předchozího cyklu. Můžeme tak naměřit i pootočení o 1000° (rozsah je pro úhlovou rychlost, ne pro úhel!).

// ú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 jsou jednoduše názvy úhlů, které jsou zažité v letecké terminologii. V kódu by se dal např. roll vypočítat takto: roll = gyroAngleX + GyroX * elapsedTime. V kódu jsme vzali 96% z gyroskopu a 4% z akcelerometru pro určení úhlů roll a pitch, proto to vypadá trošku jinak.

Zdroj obrázku: howtomechatro­nics.com

Celkové zrychlení a celkové přetížení vypočítáme jako vektorový součet zrychlení takto:

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

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

Možná se ptáte, proč je tam to číslíčko 1.03? Pokud je modul v klidu, senzor ve směru osy x a y v ideálním případě naměří zrychlení 0. Ve směru osy z působí však tíhové zrychlení i když je modul v klidu (vzpomeňte si na pohyblivé tělo na pružinách). AccZ je přibližně 1 g. Experimentálně jsem zjistil, že mi to zhruba vychází na 1.03 pro celkové zrychlení, viz obrázek:

Zdroj obrázku: howtomechatro­nics.com

Závěr

Senzor reaguje opravdu s dobrou odezvou a citlivost je vysoká. Senzor jsem položil na stůl a dokáže detekovat např. psaní na klávesnici nebo bouchání do stolu:

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);
}

V příští lekci, { NEXT}, se naučíme vyřešit problém s limity 20mA nebo 5V pro pin.

 

Měl jsi s čímkoli problém? Stáhni si vzorovou aplikaci níže a porovnej ji se svým projektem, chybu tak snadno najdeš.