#include // knihovna pro I2C komunikaci #define I2C_ADDR_NAKLON 0x68 // I2C adresa modulu mereni naklonu MPU-6050 #define dbg // komlpetni vypis informaci pri kalibraci nebo jen jednoduchy bargraf (pri zakomentovani radky) #define verze "30.7.2022 / A" // program muze byt behem testovani upravovan, proto se do vypisu zobrazuje i verze int16_t aXp, aYp, aZp; // zprumerovane tihove zrychleni v kazde ose int16_t offset_x, offset_y, offset_z; // hodnoty offsetu pro kalibraci nulove polohy int16_t OFFS_write; // hodnota pro zapis do jednotlivych offsetovych registru int32_t X_bod_K_a, Y_bod_K_a, Z_bod_K_a; // hodnota zrychleni pro body K ve vsech osach int32_t X_bod_L_a, Y_bod_L_a, Z_bod_L_a; // hodnota zrychleni pro body L ve vsech osach int32_t X_bod_K_O, Y_bod_K_O, Z_bod_K_O; // hodnota offsetu pro body K ve vsech osach int32_t X_bod_L_O, Y_bod_L_O, Z_bod_L_O; // hodnota offsetu pro body L ve vsech osach int16_t nak_cyklus; // pomocna promenna pro ruzne smycky float nak_kalibrace0; // slouzi k premapovani uhlu, ktery byl spocteny pri horizontalni pozici na 0 stupnu float nak_kalibrace90; // slouzi k premapovani uhlu, ktery byl spocteny pri vertikallni pozici na 90 stupnu int16_t znamenko; // jen uprava znamenka uhlu natoceni pro pripad, ze by se kalibrace svisle polohy provadela opacne, nez je planovano //---------------------------------------------- void setup(void) { Serial.begin(9600); Wire.begin(); #ifdef dbg Serial.print("Verze programu: "); Serial.println(verze); #endif naklon_setup(); // zakladni nastaveni cidla naklonu kalibrace_naklon(); // kalibrace offsetu a krajnich bodu naklonomeru #ifdef dbg test_45_stupnu(); // testovaci vypis 10 vzorku pri naklonu na 45 stupnu (polovina rozsahu) Serial.println(F("\n\nPredchozi vypisy zaslete prosim do fora, nebo e-mailem (krupa@centrum.cz)")); Serial.println(F("\nPokud dobehla kalibrace do konce, melo by to po odklepnuti zacit merit jako normalni vodovaha")); klik(); // cekani na odkliknuti (cekani na prijem nejakeho znaku ze seriove linky) #endif } //---------------------------------------------- //---------------------------------------------- void loop(void) { Serial.print("naklon: "); Serial.print(uhel()); Serial.print(" aXp: "); Serial.print(aXp); Serial.print(" aYp: "); Serial.print(aYp); Serial.print(" aZp: "); Serial.println(aZp); delay(200); } //---------------------------------------------- //---------------------------------------------- // pocatecni nastaveni naklonomeru vcetne dat z posledni kalibrace void naklon_setup(void) { Wire.beginTransmission (I2C_ADDR_NAKLON); // pocatecni reset a zakazani teplomeru Wire.write (0x6B); Wire.write (0b00001000); Wire.endTransmission (true); delay(100); Wire.beginTransmission (I2C_ADDR_NAKLON); // nastaveni nejcitlivejsiho rozsahu zrychleni (+/- 2g) Wire.write (0x1C); Wire.write(0x00); Wire.endTransmission (true); delay(20); Wire.beginTransmission (I2C_ADDR_NAKLON); // filtrace signalu (snizeni sumu) Wire.write (0x1D); Wire.write(0b00000110); Wire.endTransmission (true); //******************************************************************************** // // tady by se pripadne z EEPROM nacetly kalibracni hodnoty krajnich bodu a offsety, // aby nebylo nutne provadet kalibraci pri kazdem zapnuti napajeni // // int (16 bit): offset_x, offset_y, offset_z, znamenko // float: nak_kalibrace90, nak_kalibrace0 //******************************************************************************** } //---------------------------------------------- //---------------------------------------------- // vypocet uhlu naklonu pri otaceni kolem osy Y float uhel(void) { prumer(50); // do globalnich promennych aXp, aYp a aZp ulozi prumernou hodnotu zrychleni z 50 mereni float uhel_Y = atan2(aZp, sqrt(square(aXp) + square(aYp)))/(PI/180); float vysledek = mapfloat(uhel_Y, nak_kalibrace0 , nak_kalibrace90 , 0 , 90); // premapovani: Zenit = 90, horizont = 0. Z pohledu cidla jsou tyto 2 hodnoty prohozene, proto se musi premapovat if (aXp <= 0) vysledek = - vysledek; // osetreni zapornych uhlu (namireni krabicky pod horizont) vysledek = znamenko * vysledek; // osetreni opacneho naklonu pri kalibraci svisle polohy return vysledek; } //---------------------------------------------- //---------------------------------------------- // prumerovani nekolika vzorku zrychleni ve vsech osach // vysledek je ulozeny v globalnich promennych aXp, aYp a aZp void prumer(byte pocet_vzorku) { int32_t sumaX = 0; int32_t sumaY = 0; int32_t sumaZ = 0; byte smycka; for (smycka = 0; smycka < pocet_vzorku; smycka++) // prumerovani nekolika hodnot { Wire.beginTransmission (I2C_ADDR_NAKLON); Wire.write (0x3B); // uvodni registr pro zrychleni v ose X (2 bajty) Wire.endTransmission (false); delayMicroseconds(10); Wire.requestFrom (I2C_ADDR_NAKLON, 6, true); // celkem 6 registru (2 pro kazdou osu) delayMicroseconds(10); int16_t pomprom_xyz = (Wire.read() << 8 | Wire.read()); // aktualni hodnoty se nascitavaji do sumarnich promennych sumaX = sumaX + pomprom_xyz; pomprom_xyz = (Wire.read() << 8 | Wire.read()); sumaY = sumaY + pomprom_xyz; pomprom_xyz = (Wire.read() << 8 | Wire.read()); sumaZ = sumaZ + pomprom_xyz; } aXp = sumaX / pocet_vzorku; // zprumerovane hodnoty do globalnich promennych aYp = sumaY / pocet_vzorku; aZp = sumaZ / pocet_vzorku; delay(80); } //---------------------------------------------- //---------------------------------------------- // Automaticka kalibrace naklonomeru // Probehne nastaveni akcelerometrovych offsetu ve vsech trech osach pro horizontalni polohu. // Na zaver se z prectenych hodnot zrychleni vypocte uhel, ktery se bude premapovavat na 0 stupnu. // Pak se SQM natoci do zenitu a zmeri se druhy uhel, ktery se bude premapovavat na 90 stupnu. void kalibrace_naklon(void) { byte X_konec=0, Y_konec=0, Z_konec=0; // ktery konec kalibracni usecky se hleda (0 = prvni konec, 1 = druhy konec, 2 = hotovo, dal se uz nehleda) #ifdef dbg Serial.println(F("Tovarni hodnoty offsetovych registru v MPU6050 (zajima me jen prvni cteni po zapnuti napajeni):")); Serial.println(F("--------------------")); Wire.beginTransmission (I2C_ADDR_NAKLON); Wire.write (6); // uvodni registr pro OFFSET X Wire.endTransmission (false); delayMicroseconds(10); Wire.requestFrom (I2C_ADDR_NAKLON, 6, true); // celkem 6 registru OFFSETU (2 pro kazdou osu) delayMicroseconds(10); for (nak_cyklus = 0 ; nak_cyklus < 6 ; nak_cyklus ++) { Serial.print(" Registr: "); Serial.print(nak_cyklus + 6); Serial.print(" = "); Serial.println(Wire.read()); } #endif Serial.println(F("\nNastav cidlo horizontalne (napis na senzoru smeruje vzhuru)")); klik(); // cekani na odkliknuti (cekani na prijem nejakeho znaku ze seriove linky) //-------------------- FAZE 1 ----------------------- // do vsech registru se budou zapisovat vzrustajici offsety a bude se hledat takova situace, kdy zrychleni v osach x a y prekroci // hodnoty 4000 a 14000 (osa z je trochu jina, tam se hleda, kdy zrychleni prekroci hodnoty 12000 a 20000) // Vysledkem teto faze jsou pro kazdy smer 2 body (K,L) primky. #ifndef dbg Serial.println("[ | | | ]"); // jenom bargraf pod kterym se kazdych 400 cisel zobrazi hvezdicka Serial.print('['); #else Serial.print("T: millis() = "); Serial.println(millis()); #endif for (nak_cyklus = -4000; nak_cyklus < 4000; nak_cyklus = nak_cyklus + 50) // nejdriv se na hrubo najde takovy offset, pri kterem prekroci vystupy aXp a aYp hodnoty 4000 a 14000 { // (pro osu z se testuje vystup aZp na hodnoty 12000 a 20000) if (nak_cyklus < 0 ) OFFS_write = nak_cyklus - 256; // pro zaporne hodnoty zapisovanych offsetu se musi odecist cislo 256 else OFFS_write = nak_cyklus; // Zapis hodnoty offsetu do vnitrnich registru cipu MPU6050 if (X_konec < 2) zapis_offset(OFFS_write , 6); // offset X se zapisuje jen tak dlouho, nez jsou nalezene oba body if (Y_konec < 2) zapis_offset(OFFS_write , 8); // offset Y se zapisuje jen tak dlouho, nez jsou nalezene oba body if (Z_konec < 2) zapis_offset(OFFS_write , 10); // offset Z se zapisuje jen tak dlouho, nez jsou nalezene oba body if (X_konec + Y_konec + Z_konec != 6) // mereni a vypocty se provadi jed do te doby, dokud nejsou nalezeny oba body ve vsech osach { prumer(50); } if (nak_cyklus % 400 == 0) { #ifdef dbg Serial.print("Offset= "); Serial.print(OFFS_write); Serial.print(" aXp= "); Serial.print(aXp); Serial.print(" aYp= "); Serial.print(aYp); Serial.print(" aZp= "); Serial.println(aZp); #else Serial.print('*'); #endif } if (aXp > 4000 and X_konec == 0) // hledani prvniho konce usecky (bod 'K') pro osu X { X_bod_K_a = aXp; X_bod_K_O = OFFS_write; X_konec = 1; } if (aXp > 14000 and X_konec == 1) // hledani druheho konce usecky (bod 'L') pro osu X { X_bod_L_a = aXp; X_bod_L_O = OFFS_write; X_konec = 2; } if (aYp > 4000 and Y_konec == 0) // hledani prvniho konce usecky (bod 'K') pro osu Y { Y_bod_K_a = aYp; Y_bod_K_O = OFFS_write; Y_konec = 1; } if (aYp > 14000 and Y_konec == 1) // hledani druheho konce usecky (bod 'L') pro osu Y { Y_bod_L_a = aYp; Y_bod_L_O = OFFS_write; Y_konec = 2; } if (aZp > 12000 and Z_konec == 0) // hledani prvniho konce usecky (bod 'K') pro osu Z (proti osam X a Y jsou tady body trochu posunute) { Z_bod_K_a = aZp; Z_bod_K_O = OFFS_write; Z_konec = 1; } if (aZp > 20000 and Z_konec == 1) // hledani druheho konce usecky (bod 'L') pro osu Z { Z_bod_L_a = aZp; Z_bod_L_O = OFFS_write; Z_konec = 2; } } #ifndef dbg Serial.print('|'); #endif if (X_konec + Y_konec + Z_konec != 6) // nebylo nalezeno vsech 6 bodu { Serial.println("Chyba kalibrace naklonu"); klik(); // cekani na odkliknuti (cekani na prijem nejakeho znaku ze seriove linky) return; // konec podprogramu - chyba autokalibrace } //-------------------- FAZE 2 ----------------------- // ze dvou znamych bodu kazde usecky se nahrubo vypocte takovy offset, pri kterem usecka protina osu x (vystup zrychleni je 0) // vychazi se z obecne rovnice primky y = ax + b float a = (float)(X_bod_L_a - X_bod_K_a) / (X_bod_L_O - X_bod_K_O); float b = X_bod_K_a - (X_bod_K_O * a); float TeorOffX = -b / a; // 'TeorOffX' je teoreticka hodnota ve ktere by mela primka mezi body K a L protinat osou x (hodnota na ose y = 0) #ifdef dbg Serial.print("\nT: millis() = "); Serial.println(millis()); Serial.print("\nxK = ["); Serial.print(X_bod_K_O); Serial.print(','); Serial.print(X_bod_K_a); Serial.println(']'); Serial.print("xL = ["); Serial.print(X_bod_L_O); Serial.print(','); Serial.print(X_bod_L_a); Serial.println(']'); Serial.print("yK = ["); Serial.print(Y_bod_K_O); Serial.print(','); Serial.print(Y_bod_K_a); Serial.println(']'); Serial.print("yL = ["); Serial.print(Y_bod_L_O); Serial.print(','); Serial.print(Y_bod_L_a); Serial.println(']'); Serial.print("zK = ["); Serial.print(Z_bod_K_O); Serial.print(','); Serial.print(Z_bod_K_a); Serial.println(']'); Serial.print("zL = ["); Serial.print(Z_bod_L_O); Serial.print(','); Serial.print(Z_bod_L_a); Serial.println(']'); Serial.print("\nTeorOffX:"); Serial.println(TeorOffX); #else Serial.print('*'); #endif a = (float)(Y_bod_L_a - Y_bod_K_a) / (Y_bod_L_O - Y_bod_K_O); b = Y_bod_K_a - (Y_bod_K_O * a); float TeorOffY = -b / a; // 'TeorOffY' je teoreticka hodnota ve ktere by mela primka mezi body K a L protinat osou x (hodnota na ose y = 0) #ifdef dbg Serial.print("TeorOffY:"); Serial.println(TeorOffY); #else Serial.print('*'); #endif a = (float)(Z_bod_L_a - Z_bod_K_a) / (Z_bod_L_O - Z_bod_K_O); // osa Z se nastavuje tak, aby mela v horizontalni poloze hodnotu 16384, proto je vypocet trochu jiny b = Z_bod_K_a - (Z_bod_K_O * a); float TeorOffZ = (16384-b) / a; // 'TeorOffZ' je teoreticka hodnota ve ktere by mela mit primka mezi body K a L hodnotu 16384 na ose y #ifdef dbg Serial.print("TeorOffZ:"); Serial.println(TeorOffZ); Serial.print("\n\n"); #else Serial.print("*|"); #endif //-------------------- FAZE 3 ----------------------- // Tady uz jede kazda osa ve sve vlastni smycce. // ke kazdemu offsetu se prida na hrubo nekolik desitek cisel, aby bylo zajisteno, ze vysledne zrychleni bude bezpecne kladne (nebo nad hodnotou 16384 pro osu Z) nak_cyklus = TeorOffX; // zacina se teoreticky vypoctenou hodnotou offsetu pro osu X do // smycka musi projit alespon 1x { nak_cyklus = nak_cyklus + 20; // k offsetu se pricita nekolik desitek cisel, dokud neni vystup zarucene kladny zapis_offset(nak_cyklus , 6); prumer(50); #ifdef dbg Serial.print("X: offset: "); Serial.print(nak_cyklus); Serial.print(" aXp: "); Serial.println(aXp); #endif } while (aXp < 100); offset_x = nak_cyklus; // pomocna promenna pro osu X urcuje, od ktereho cisla se bude v nasledujici fazi pokracovat s jemnym doladenim #ifndef dbg Serial.print('*'); #endif nak_cyklus = TeorOffY; // zacina se teoreticky vypoctenou hodnotou offsetu pro osu Y do // smycka musi projit alespon 1x { nak_cyklus = nak_cyklus + 20; // k offsetu se pricita nekolik desitek cisel, dokud neni vystup zarucene kladny zapis_offset(nak_cyklus , 8); prumer(50); #ifdef dbg Serial.print("Y: offset: "); Serial.print(nak_cyklus); Serial.print(" aYp: "); Serial.println(aYp); #endif } while (aYp < 100); offset_y = nak_cyklus; // pomocna promenna pro osu X urcuje, od ktereho cisla se bude v nasledujici fazi pokracovat s jemnym doladenim #ifndef dbg Serial.print('*'); #endif nak_cyklus = TeorOffZ; // zacina se teoreticky vypoctenou hodnotou offsetu pro osu Z do // smycka musi projit alespon 1x { nak_cyklus = nak_cyklus + 20; // k offsetu se pricita nekolik desitek cisel, dokud neni vystup zarucene kladny zapis_offset(nak_cyklus , 10); prumer(50); #ifdef dbg Serial.print("Z: offset: "); Serial.print(nak_cyklus); Serial.print(" aZp: "); Serial.println(aZp); #endif } while (aZp < 16500); offset_z = nak_cyklus; // pomocna promenna pro osu Z urcuje, od ktereho cisla se bude v nasledujici fazi pokracovat s jemnym doladenim #ifndef dbg Serial.print("*|"); #endif //-------------------- FAZE 4 ----------------------- // Az sem se hodnota offsetu urcovala s velkou rychlosti, ale na hrubo. // Od ted uz se POMALU odecita od offsetu jednicka tak dlouho, nez se merene zrychleni dostane pod 0 v osach X a Y (nebo pod 16348 na ose Z) #ifdef dbg Serial.print("\nT: millis() = "); Serial.println(millis()); Serial.print('\n'); #endif nak_cyklus = offset_x; while (aXp > 0) // pomale priblizovani k 0 na ose X zhora { nak_cyklus --; offset_x = nak_cyklus; zapis_offset(offset_x , 6); prumer(50); #ifdef dbg Serial.print("X: offset: "); Serial.print(nak_cyklus); Serial.print(" aXp: "); Serial.println(aXp); #endif } #ifndef dbg Serial.print('*'); #endif nak_cyklus = offset_y; while (aYp > 0) // pomale priblizovani k 0 na ose Y zhora { nak_cyklus --; offset_y = nak_cyklus; zapis_offset(offset_y , 8); prumer(50); #ifdef dbg Serial.print("Y: offset: "); Serial.print(nak_cyklus); Serial.print(" aYp: "); Serial.println(aYp); #endif } #ifndef dbg Serial.print('*'); #endif nak_cyklus = offset_z; while (aZp > 16384) // pomale priblizovani k 16384 na ose Z zhora { nak_cyklus --; offset_z = nak_cyklus; zapis_offset(offset_z , 10); prumer(50); #ifdef dbg Serial.print("Z: offset: "); Serial.print(nak_cyklus); Serial.print(" aZp: "); Serial.println(aZp); #endif } #ifndef dbg Serial.println("*]"); #endif nak_kalibrace0 = atan2(aZp, sqrt(square(aXp) + square(aYp)))/(PI/180.0); // ve vodorovne pozici se ze tri os spocte uhel, ktery bude slouzit k premapovani na 0 stupnu #ifdef dbg Serial.print("\nHoriz. : aXp : "); Serial.print(aXp); Serial.print(" aYp: "); Serial.print(aYp); Serial.print(" aZp: "); Serial.print(aZp); Serial.print(" Kal0: "); Serial.println(nak_kalibrace0); Serial.print("\nT: millis() = "); Serial.println(millis()); #endif Serial.println("\n\nOtoc cidlo kolem osy Y do svisle polohy (napis na senzoru miri do boku)"); klik(); // cekani na odkliknuti (cekani na prijem nejakeho znaku ze seriove linky) prumer(50); nak_kalibrace90 = atan2(aZp, sqrt(square(aXp) + square(aYp)))/(PI/180.0); // tento uhel bude slouzit k premapovani na 90 stupnu if (aXp < 0) znamenko = -1; // pokud by se na svislou polohu otacelo opacne, nez je ocekavano, bude se ve vysledku menit znamenko else znamenko = 1; #ifdef dbg Serial.print("Vertikal. : aXp : "); Serial.print(aXp); Serial.print(" aYp: "); Serial.print(aYp); Serial.print(" aZp: "); Serial.print(aZp); Serial.print(" Kal90: "); Serial.println(nak_kalibrace90); Serial.print("\n\nOffset_X: "); Serial.println(offset_x); Serial.print("Offset_Y: "); Serial.println(offset_y); Serial.print("Offset_Z: "); Serial.println(offset_z); Serial.println("\nOK\n"); #endif //******************************************************************************** // // tady by se kalibracni hodnoty krajnich bodu a offsety ukladaly do EEPROM, // aby nebylo nutne provadet kalibraci pri kazdem zapnuti: // // int (16 bit): offset_x, offset_y, offset_z, znamenko // float: nak_kalibrace90, nak_kalibrace0 //******************************************************************************** } //---------------------------------------------- //---------------------------------------------- // zapis dvojbajtoveho cisla do zadaneho registru (a registru +1) MPU6050 void zapis_offset( int16_t hodnota, int16_t registr) { Wire.beginTransmission (I2C_ADDR_NAKLON); Wire.write (registr); Wire.write(hodnota / 256); Wire.endTransmission (true); delay(10); Wire.beginTransmission (I2C_ADDR_NAKLON); Wire.write (registr +1); Wire.write(hodnota % 256); Wire.endTransmission (true); delay(10); } //---------------------------------------------- //---------------------------------------------- // druha mocnina pro velka cisla int32_t square(int16_t vstup) { return 1UL * vstup * vstup; } //---------------------------------------------- //---------------------------------------------- // nahrada funkce map() pro desetinna cisla float mapfloat(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } //---------------------------------------------- //---------------------------------------------- // cekani na odeslani nejakeho znaku do seriove linky void klik(void) { Serial.println(">>"); while (Serial.available() == 0) delay(100); // cekani na odklepnuti while(Serial.available()) Serial.read() ; delay(100); } //---------------------------------------------- //---------------------------------------------- void test_45_stupnu(void) { Serial.println(F("Test na polovine rozsahu")); Serial.println(F("\nNaklon senzor na 45 stupnu (podlozit trojuhelnikem)")); klik(); Serial.println(F("--- 10x mereni pro 45 stupnu ---")); for (nak_cyklus = 0; nak_cyklus < 10 ; nak_cyklus ++) // jen 10x vypis aktualnich hodnot zrychleni ve vsech osach { Serial.print("naklon: "); // vypocteny naklon Serial.print(uhel()); Serial.print(" aXp: "); // vypis registru pro zrychleni Serial.print(aXp); Serial.print(" aYp: "); Serial.print(aYp); Serial.print(" aZp: "); Serial.println(aZp); delay(200); } } //----------------------------------------------