MPU 6050 Gyro, Արագինաչափ Կապ Arduino- ի հետ (Atmega328p) ՝ 5 քայլ

2024 Հեղինակ: John Day | [email protected]. Վերջին փոփոխված: 2024-01-30 09:45

MPU6050 IMU- ն ունի ինչպես 3-առանցքի արագացուցիչ, այնպես էլ 3-առանցքային գիրոսկոպ, որոնք ինտեգրված են մեկ չիպի վրա:

Գիրոսկոպը չափում է ժամանակի ընթացքում անկյունային դիրքի պտտման արագությունը կամ արագությունը ՝ X, Y և Z առանցքների երկայնքով:

Գիրոսկոպի ելքերը գտնվում են վայրկյանում աստիճաններով, ուստի անկյունային դիրքը ստանալու համար մեզ պարզապես անհրաժեշտ է ինտեգրել անկյունային արագությունը:

Մյուս կողմից, MPU6050 արագացուցիչը չափում է արագացումը `չափելով գրավիտացիոն արագացումը 3 առանցքների երկայնքով և որոշ եռանկյունաչափական մաթեմատիկայի միջոցով կարող ենք հաշվարկել այն անկյունը, որտեղ սենսորը տեղադրված է: Այսպիսով, եթե մենք միացնենք կամ միավորենք արագացուցիչի և գիրոսկոպի տվյալները, կարող ենք շատ ճշգրիտ տեղեկատվություն ստանալ սենսորի կողմնորոշման մասին:

MPU-6050- ը բաղկացած է 3 առանցքի գիրոսկոպից, որը կարող է հայտնաբերել պտտման արագությունը x, y, z առանցքի երկայնքով միկրոէլեկտրամեխանիկական համակարգի տեխնոլոգիայով (MEMS): Երբ սենսորը պտտվում է ցանկացած առանցքի երկայնքով, Coriolis- ի էֆեկտի պատճառով առաջանում է թրթռում, որը հայտնաբերվում է MEMS- ով: 16-բիթանոց ADC- ն օգտագործվում է յուրաքանչյուր առանցքի նմուշառման համար լարումը թվայնացնելու համար: +/- 250, +/- 500, +/- 1000, +/- 2000-ը արտադրանքի ամբողջ մասշտաբն է: Անկյունային արագությունը չափվում է յուրաքանչյուր առանցքի երկայնքով մեկ վայրկյանում:

Օգտակար հղում ՝ ……………

Arduino տախտակ. ……….

MPU6050 IMU ……………

Քայլ 1: MPU-6050 մոդուլ

MPU-6050 մոդուլն ունի 8 կապում,

INT: Ընդհատել թվային ելքային քորոցը:

AD0: I2C Slave Հասցե LSB քորոց: Սա 0-րդ բիթն է սարքի 7-բիթ ստրուկ հասցեում: Եթե միացված է VCC- ին, ապա այն կարդացվում է որպես տրամաբանական մեկ և փոխվում է ստրուկի հասցեն:

XCL: Օժանդակ սերիական ժամացույցի քորոց: Այս քորոցն օգտագործվում է I2C ինտերֆեյսի միջոցով սենսորների SCL կապը MPU-6050- ին միացնելու համար:

XDA: Օժանդակ սերիական տվյալների քորոց: Այս քորոցը օգտագործվում է I2C ինտերֆեյսի միջոցով սենսորների SDA քորոցը MPU-6050- ին միացնելու համար:

SCL: Սերիայի ժամացույցի քորոց: Միացրեք այս կապը միկրոկառավարիչների SCL կապին: SDA: Սերիական տվյալների քորոց: Միացրեք այս կապը միկրոկառավարիչների SDA քորոցին:

GND: Աղացած քորոց: Միացրեք այս քորոցը գետնին:

VCC: Էներգամատակարարման քորոց: Միացրեք այս կապը +5V DC հոսանքի աղբյուրին: MPU-6050 մոդուլն ունի Slave հասցե (երբ AD0 = 0, այսինքն ՝ այն միացված չէ Vcc- ին), ինչպես, Ստրուկ Գրեք հասցեն (SLA+W) ՝ 0xD0

Ստրուկ Կարդալու հասցեն (SLA+R) ՝ 0xD1

Քայլ 2: Հաշվարկներ

MPU6050 մոդուլի գիրոսկոպի և արագացուցիչի տվիչների տվյալները բաղկացած են 16-բիթանոց հումքային տվյալներից ՝ 2-ի լրացման տեսքով:

MPU6050 մոդուլի ջերմաստիճանի տվիչների տվյալները բաղկացած են 16-բիթանոց տվյալներից (ոչ 2-ի լրացման տեսքով):

Հիմա ենթադրենք, որ մենք ընտրել ենք,

• - Արագացուցիչի լայնածավալ ընդգրկույթ +/- 2 գ զգայունության սանդղակի գործոնով ՝ 16, 384 LSB (հաշվարկ)/գ:
• - Գիրոսկոպ +/- 250 °/վ լայնածավալ ընդգրկույթ ՝ զգայունության սանդղակի գործոնով ՝ 131 LSB (հաշվարկ)/°/վ: ապա,

Սենսորային հումքային տվյալներ ստանալու համար մենք պետք է առաջին հերթին կատարենք 2 -ի հավելումը արագացուցիչի և գիրոսկոպի տվիչների տվյալների վրա: Սենսորային հումքային տվյալներ ստանալուց հետո մենք կարող ենք հաշվարկել արագացումը և անկյունային արագությունը ՝ սենսորային հումքի տվյալները իրենց զգայունության սանդղակի գործոնով բաժանելով հետևյալ կերպ.

Արագացուցիչի արժեքները g- ում (g ուժ)

• X առանցքի երկայնքով արագացում = (Accelerometer X axis raw data/16384) գ.
• Y առանցքի երկայնքով արագացում = (Accelerometer Y axis raw data/16384) գ.
• Z առանցքի երկայնքով արագացում = (Accelerometer Z axis raw data/16384) գ.

Գիրոսկոպի արժեքները °/վ -ում (աստիճան մեկ վայրկյանում)

• Անկյունային արագություն X առանցքի երկայնքով = (գիրոսկոպ X առանցքի հում տվյալներ/131) °/վ:
• Y առանցքի երկայնքով անկյունային արագություն = (գիրոսկոպ Y առանցքի հում տվյալներ/131) °/վ:
• Z առանցքի երկայնքով անկյունային արագություն = (Gyroscope Z առանցքի տվյալներ/131) °/վ:

Peratերմաստիճանի արժեքը °/c (աստիճան ըստ Celsius)

Degreesերմաստիճանը աստիճաններով C = ((ջերմաստիճանի տվիչի տվյալներ)/340 + 36.53) °/գ:

Օրինակ, Ենթադրենք, 2’լրացումից հետո մենք ստանում ենք արագացուցաչափ X առանցքների հում արժեք = +15454

Այնուհետեւ Կացին = +15454/16384 = 0.94 գ:

Ավելին,

Այսպիսով, մենք գիտենք, որ մենք աշխատում ենք +/- 2G և +/- 250deg/վ զգայունությամբ, բայց ինչպես են մեր արժեքները համապատասխանում այդ արագացումներին/անկյուններին:

Սրանք երկուսն էլ ուղիղ գծապատկերներ են, և դրանցից կարող ենք պարզել, որ 1G- ի համար կկարդանք 16384, իսկ 1 աստիճան/վրկ համար `131.07 (չնայած.07 -ը երկուական պատճառով անտեսվելու է) այս արժեքները պարզապես մշակվել են գծապատկերով ուղիղ գծապատկեր `2G- ով` 32767 և -2G- ով `-32768 և 250/-250 նույն արժեքներով:

Այսպիսով, այժմ մենք գիտենք մեր զգայունության արժեքները (16384 և 131.07) մենք պարզապես պետք է հանենք մեր արժեքներից փոխհատուցումները և այնուհետև բաժանվենք զգայունությունից:

Սրանք լավ կաշխատեն X և Y արժեքների համար, բայց քանի որ Z- ն գրանցվել է 1G- ում և ոչ 0 -ի դեպքում, մեզ անհրաժեշտ կլինի մինուս 1G- ից (16384), նախքան մեր զգայունությամբ բաժանվելը:

Քայլ 3. MPU6050-Atmega328p միացումներ

Պարզապես միացրեք ամեն ինչ, ինչպես նշված է գծապատկերում:…

Կապերը տրվում են հետևյալ կերպ.

MPU6050 Arduino Nano:

VCC 5v ելքային քորոց

GND Գրունտի քորոց

SDA A4 փին // սերիական տվյալներ

SCL A5 փին // սերիական ժամացույց

Պտույտի և գլորման հաշվարկ. Պտույտը պտույտն է x առանցքի շուրջը, իսկ սկիպիդարը `պտտումը y առանցքի երկայնքով:

Արդյունքը ռադիոներում է: (փոխակերպեք աստիճանների `բազմապատկելով 180 -ով և բաժանելով pi- ով)

Քայլ 4: Կոդեր և բացատրություններ

/*

Arduino և MPU6050 արագացուցիչ և գիրոսկոպ սենսորների ձեռնարկ ՝ Dejan- ի կողմից, https://howtomechatronics.com */#include const int MPU = 0x68; // MPU6050 I2C հասցե float AccX, AccY, AccZ; բոց GyroX, GyroY, GyroZ; float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; բոց գլորում, սկիպիդար, հորանջում; float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; float elapsedTime, currentTime, previousTime; int c = 0; void setup () {Serial.begin (19200); Wire.begin (); // Initialize հաղորդակցության Wire.beginTransmission (MPU); // Սկսեք հաղորդակցություն MPU6050- ի հետ // MPU = 0x68 Wire.write (0x6B); // Խոսեք գրանցամատյանի հետ 6B Wire.write (0x00); // Կատարել վերականգնում - տեղադրել 0 -ը 6B գրանցամատյանում Wire.endTransmission (ճշմարիտ); // ավարտել փոխանցումը/* // Կարգավորել արագացուցիչի զգայունություն - Լայնածավալ ընդգրկույթ (կանխադրված +/- 2 գ) Wire.beginTransmission (MPU); Wire.write (0x1C); // Խոսեք ACCEL_CONFIG գրանցամատյանի հետ (1C hex) Wire.write (0x10); // Գրանցման բիթերը սահմանել 00010000 (+/- 8 գ ամբողջական մասշտաբի տիրույթ) Wire.endTransmission (ճշմարիտ); // Կարգավորել Gyro Sensitivity - Full Scale Range (կանխադրված +/- 250deg/վ) Wire.beginTransmission (MPU); Wire.write (0x1B); // Խոսեք GYRO_CONFIG գրանցամատյանի հետ (1B վեցանկյուն) Wire.write (0x10); // Գրանցիչի բիթերը սահմանել 00010000 (1000deg/վ լրիվ մասշտաբով) Wire.endTransmission (ճշմարիտ); ուշացում (20); */ // Callանգահարեք այս գործառույթը, եթե Ձեզ անհրաժեշտ է ստանալ IMU սխալի արժեքները ձեր մոդուլին calc_IMU_error (); ուշացում (20); } void loop () {// === Կարդալ արագացուցիչի տվյալները === // Wire.beginTransmission (MPU); Wire.write (0x3B); // Սկսեք գրանցամատյանով 0x3B (ACCEL_XOUT_H) Wire.endTransmission (false); Հաղորդալար. Խնդրում ենք (MPU, 6, ճշմարիտ); // Ընդհանուր կարդալ 6 գրանցամատյան, յուրաքանչյուր առանցքի արժեքը պահվում է 2 գրանցամատյանում // +-2 գ միջակայքի համար մենք պետք է չմշակված արժեքները բաժանենք 16384-ի, համաձայն տվյալների թերթիկի ՝ AccX = (Wire.read () << 8 | Wire.read ()) / 16384.0; // X առանցքի արժեքը AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Y- առանցքի արժեքը AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Z- առանցքի արժեքը // Շրջագայության և սկիպիդարի հաշվարկ արագացուցիչի տվյալներից accAngleX = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~ (0.58) Մանրամասների համար տե՛ս calc_IMU_error () հատուկ գործառույթը accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~ (-1.58) // === Ընթերցել գիրոսկոպի տվյալները === // previousTime = currentTime; // Նախորդ ժամանակը պահվում է մինչ իրական ժամանակը կարդալ currentTime = millis (); // Ընթացիկ ժամանակը փաստացի ժամանակը կարդալ elapsedTime = (currentTime - previousTime) / 1000; // Բաժանեք 1000 -ի ՝ վայրկյաններ ստանալու համար Wire.beginTransmission (MPU); Wire.write (0x43); // Gyro data առաջին գրանցման հասցեն 0x43 Wire.endTransmission (false); Հաղորդալար. Խնդրում ենք (MPU, 6, ճշմարիտ); // Ընդհանուր կարդալ 4 գրանցամատյան, յուրաքանչյուր առանցքի արժեքը պահվում է 2 գրանցամատյանում GyroX = (Wire.read () << 8 | Wire.read ()) / 131.0; // 250 աստիճան/ վրկ միջակայքի համար մենք պետք է նախ հում արժեքը բաժանենք 131.0 -ի, ըստ GyroY = (Wire.read () << 8 | Wire.read ())/ 131.0 տվյալների թերթիկի: GyroZ = (Wire.read () << 8 | Wire.read ()) / 131.0; // Արդյունքները շտկել հաշվարկված սխալի արժեքներով GyroX = GyroX + 0.56; // GyroErrorX (-0.56) GyroY = GyroY - 2; // GyroErrorY (2) GyroZ = GyroZ + 0.79; // GyroErrorZ (-0.8) // Ներկայումս հումքի արժեքները գտնվում են վայրկյանում աստիճանների վրա, deg/s, այնպես որ մենք պետք է բազմապատկենք sendonds (ներ) ով, որպեսզի անկյունը ստանանք gyroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg gyroAngleY = gyroAngleY + GyroY * անցած ժամանակ; yaw = yaw + GyroZ * elapsedTime; // Լրացուցիչ զտիչ - միավորել արագացուցիչի և գիրո անկյունների արժեքները գլորում = 0.96 * gyroAngleX + 0.04 * accAngleX; սկիպիդար = 0.96 * gyroAngleY + 0.04 * accAngleY; // Տպել արժեքները սերիական մոնիտորի Serial.print (roll); Serial.print ("/"); Serial.print (սկիպիդար); Serial.print ("/"); Serial.println (yaw); } void calc_IMU_error () {// Այս գործառույթը կարող ենք անվանել կարգաբերման բաժնում `արագացուցիչի և գիրոսկոպի սխալը հաշվարկելու համար: Այստեղից մենք կստանանք Սերիական մոնիտորի վրա տպված վերը նշված հավասարումների մեջ օգտագործված սխալի արժեքները: // Նկատի ունեցեք, որ մենք պետք է տեղադրենք IMU- ն հարթ, որպեսզի ստանանք համապատասխան արժեքներ, որպեսզի կարողանանք ճիշտ արժեքներ ստանալ // կարդալ արագացուցիչի արժեքները 200 անգամ, մինչդեռ (c <200) {Wire.beginTransmission (MPU); Wire.write (0x3B); Wire.endTransmission (կեղծ); Հաղորդալար. Խնդրում ենք (MPU, 6, ճշմարիտ); AccX = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Ամփոփեք բոլոր ընթերցումները 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 ++; } // Գումարը բաժանեք 200 -ի ՝ սխալի արժեքը ստանալու համար AccErrorX = AccErrorX /200; AccErrorY = AccErrorY / 200; c = 0; // Կարդացեք գիրոյի արժեքները 200 անգամ, մինչդեռ (c <200) {Wire.beginTransmission (MPU); Wire.write (0x43); Wire.endTransmission (կեղծ); Հաղորդալար. Խնդրում ենք (MPU, 6, ճշմարիտ); 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 ++; } // Գումարը բաժանեք 200 -ի ՝ սխալի արժեքը ստանալու համար GyroErrorX = GyroErrorX /200; GyroErrorY = GyroErrorY / 200; GyroErrorZ = GyroErrorZ / 200; // Տպել սխալի արժեքները Serial Monitor 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); } ----------------------------------------------------- -------------------------------------------------- արդյունքներ.-X = Y = Z = ------------------------------------------------- -------------------------------------------------- Կարևոր նշում. -----------------

Օղակի բաժնում մենք սկսում ենք կարդալ արագացուցիչի տվյալները: Յուրաքանչյուր առանցքի տվյալները պահվում են 2 բայթ կամ գրանցամատյանում, և մենք կարող ենք տեսնել այդ գրանցամատյանների հասցեները սենսորի տվյալների թերթիկից:

Բոլորը կարդալու համար մենք սկսում ենք առաջին գրանցամատյանից, և օգտագործելով requiestFrom () գործառույթը, մենք խնդրում ենք կարդալ բոլոր 6 գրանցամատյանները X, Y և Z առանցքների համար: Այնուհետև մենք կարդում ենք յուրաքանչյուր գրանցամատյանի տվյալները, և քանի որ արդյունքները երկուսն են լրացնում, մենք դրանք համապատասխանաբար համատեղում ենք ՝ ճիշտ արժեքներ ստանալու համար:

Քայլ 5. Հասկանալով թեքության անկյունը

Արագացուցիչ

Երկրի ձգողականությունը մշտական արագացում է, որտեղ ուժը միշտ ուղղված է դեպի Երկրի կենտրոնը:

Երբ արագացուցիչը ձգողականության հետ զուգահեռ է, չափված արագացումը կլինի 1G, երբ արագացուցիչը ձգողության հետ ուղղահայաց է, այն չափելու է 0G:

Թեքության անկյունը կարող է հաշվարկվել չափված արագացումից ՝ օգտագործելով այս հավասարումը.

θ = մեղք -1 (չափված արագացում / ձգողականության արագացում)

GyroGyro (a.k.a. սենսոր) օգտագործվում է անկյունային արագությունը (ω) չափելու համար:

Ռոբոտի թեքության անկյունը ստանալու համար մենք պետք է ինտեգրենք տվյալները գիրոնից, ինչպես ցույց է տրված ստորև բերված հավասարման մեջ.

ω = dθ / dt, θ = ∫ ω dt

Գիրո և արագացուցիչի սենսորային միաձուլում Հետազոտելով թե՛ գիրո, թե՛ արագացուցիչի բնութագիրը, մենք գիտենք, որ դրանք ունեն իրենց ուժեղ և թույլ կողմերը: Արագացուցիչի տվյալների հաշվարկված թեքության անկյունն ունի դանդաղ արձագանքման ժամանակ, մինչդեռ գիրոսկրային տվյալներից թեքության անկյունը որոշակի ժամանակ ենթարկվում է շեղման: Այլ կերպ ասած, մենք կարող ենք ասել, որ արագացուցիչի տվյալները օգտակար են երկարաժամկետ հեռանկարում, մինչդեռ գիրոս տվյալները կարճաժամկետ են:

Հղում ավելի լավ հասկանալու համար. Սեղմեք այստեղ