MPU 6050 Gyro, Arduino bilan akselerometr aloqasi (Atmega328p): 5 qadam

2024 Muallif: John Day | [email protected]. Oxirgi o'zgartirilgan: 2024-01-30 13:23

MPU6050 IMUda bitta o'qga o'rnatilgan 3 o'qli akselerometr va 3 o'qli giroskop mavjud.

Gyroskop X, Y va Z o'qlari bo'ylab vaqt o'tishi bilan burchak holatining aylanish tezligini yoki o'zgarish tezligini o'lchaydi.

Gyroskopning chiqishlari sekundiga gradusli, shuning uchun burchak holatini olish uchun biz faqat burchak tezligini birlashtirishimiz kerak.

Boshqa tomondan, MPU6050 akselerometri 3 eksa bo'ylab tortishish tezlanishini o'lchash orqali tezlikni o'lchaydi va ba'zi trigonometriya matematikasi yordamida biz sensor joylashadigan burchakni hisoblaymiz. Shunday qilib, agar biz akselerometr va giroskop ma'lumotlarini birlashtirsak yoki birlashtirsak, biz sensorning yo'nalishi haqida juda aniq ma'lumotga ega bo'lamiz.

3 o'qli giroskop MPU-6050 3 o'qli giroskopdan iborat bo'lib, u x, y, z o'qi bo'ylab aylanish tezligini mikro-mexanik tizim texnologiyasi (MEMS) yordamida aniqlay oladi. Sensor har qanday o'q bo'ylab aylantirilganda, Coriolis effekti tufayli tebranish hosil bo'ladi, u MEMS tomonidan aniqlanadi. 16-bitli ADC har bir o'qni tanlash uchun kuchlanishni raqamlashtirish uchun ishlatiladi. +/- 250, +/- 500, +/- 1000, +/- 2000- bu chiqishning to'liq ko'lamli diapazoni. Burchak tezligi har bir o'q bo'ylab sekundiga birlik darajasida o'lchanadi.

Foydali havola: …………….

Arduino taxtasi: ……….

MPU6050 IMU ……………

1-qadam: MPU-6050 moduli

MPU-6050 moduli 8 ta pinli,

INT: raqamli chiqish pinini uzish.

AD0: I2C qul manzili LSB pin. Bu qurilmaning 7-bitli qul manzilidagi 0-bit. Agar VCC ga ulangan bo'lsa, u mantiqiy va qulli manzil o'zgarishi sifatida o'qiladi.

XCL: Yordamchi seriyali soat pimi. Ushbu pin boshqa I2C interfeysli SCL pinlarini MPU-6050 ga ulash uchun ishlatiladi.

XDA: Yordamchi ketma -ket ma'lumot pin. Ushbu pin boshqa I2C interfeysli SDA pinini MPU-6050 ga ulash uchun ishlatiladi.

SCL: Seriyali soat pimi. Ushbu pinni SCL pinli mikrokontrollerlarga ulang. SDA: ketma -ket ma'lumot pin. Bu pinni mikrokontroller SDA piniga ulang.

GND: Topraklama pimi. Bu pinni yerga ulang.

VCC: quvvat manbai. Ushbu pinni +5V shahar manbaiga ulang. MPU-6050 moduli qul manziliga ega (AD0 = 0 bo'lsa, ya'ni Vcc ga ulanmagan), Qul yozish manzili (SLA+W): 0xD0

Qul o'qish manzili (SLA+R): 0xD1

2 -qadam: Hisob -kitoblar

MPU6050 modulining giroskop va akselerometr sensori ma'lumotlari 2-sonli to'ldiruvchi 16-bitli xom ma'lumotlardan iborat.

MPU6050 modulining harorat sensori ma'lumotlari 16-bitli ma'lumotlardan iborat (2-qo'shimchali shaklda emas).

Aytaylik, biz tanladik,

• - Akselerometrning to'liq o'lchov diapazoni +/- 2g, sezuvchanlik o'lchov omili 16, 384 LSB (hisoblash)/g.
• - Giroskopning to'liq o'lchov diapazoni +/- 250 °/s, sezuvchanlik o'lchov omili 131 LSB (hisoblash)/°/s. keyin,

Sensor ma'lumotlarini olish uchun biz birinchi navbatda akselerometr va giroskop sensori ma'lumotlariga 2 -sonli qo'shimchani bajarishimiz kerak. Sensor xom ma'lumotlarini olgandan so'ng, biz sezgirlik shkalasi koeffitsienti bilan sensor ma'lumotlarini bo'linib, tezlanish va burchak tezligini hisoblashimiz mumkin.

Akselerometr qiymatlari g (g kuch)

• X o'qi bo'ylab tezlanish = (X o'qi akselerometrining xom ma'lumoti/16384) g.
• Y o'qi bo'ylab tezlanish = (Y o'qi akselerometrining xom ma'lumotlari/16384) g.
• Z o'qi bo'ylab tezlanish = (akselerometr Z o'qi xom ma'lumoti/16384) g.

Giroskopning qiymatlari °/s (sekundiga daraja)

• X o'qi bo'ylab burchak tezligi = (X o'qi giroskopi ma'lumotlari/131) °/s.
• Y o'qi bo'ylab burchak tezligi = (giroskop Y o'qi xom ma'lumoti/131) °/s.
• Z o'qi bo'ylab burchak tezligi = (giroskop Z o'qi xom ma'lumoti/131) °/s.

Harorat qiymati ° C/s (Selsiy bo'yicha daraja)

Harorat C darajasida = ((harorat sensori ma'lumotlari)/340 + 36,53) °/s.

Masalan, Aytaylik, 2 -sonli to'ldirgandan so'ng biz X o'qi akselerometrini olamiz, xom qiymati = +15454

Keyin Ax = +15454/16384 = 0,94 g.

Ko'proq,

Shunday qilib, biz +/- 2G va +/- 250deg/s sezgirlik bilan ishlayotganimizni bilamiz, lekin bizning qadriyatlarimiz bu tezlanishlar/burchaklarga qanday mos keladi.

Bu ikkalasi ham to'g'ri chiziqli grafiklar va biz ulardan xulosa chiqarishimiz mumkinki, 1G uchun biz 16384 ni o'qiymiz va 1 gradus/sekund uchun biz 131.07 ni o'qiymiz (garchi.07 ikkilik tufayli e'tiborga olinmasa ham), bu qiymatlar chizilgan chizish orqali ishlab chiqilgan. 32767 da 2G va -32768 va -2G bilan bir xil qiymatlarda 250/-250 bo'lgan to'g'ri chiziqli grafik.

Shunday qilib, endi biz sezgirlik qiymatlarimizni bilamiz (16384 va 131.07), biz shunchaki qadriyatlarimizning o'rnini bosishimiz kerak, keyin esa sezgirlikni ajratishimiz kerak.

Bular X va Y qiymatlari uchun juda yaxshi ishlaydi, lekin Z 0da emas, balki 1G da yozilgandek, biz sezgirligimizga bo'linishidan oldin 1G (16384) ni o'chirishimiz kerak bo'ladi.

3-qadam: MPU6050-Atmega328p ulanishlari

Har bir narsani diagrammada ko'rsatilganidek ulang …

Ulanishlar quyidagicha berilgan:

MPU6050 Arduino Nano

VCC 5V chiqish pimi

GND tuproqli pin

SDA A4 pinli // ketma -ket ma'lumotlar

SCL A5 pin // ketma -ket soat

Pitch va Roll hisoblash: Roll-bu x o'qi atrofida aylanish va y o'qi bo'ylab aylanish.

Natijada radianlarda bo'ladi. (180 ga ko'paytirish va pi ga bo'lish orqali darajalarga aylantiring)

4 -qadam: Kodlar va tushuntirishlar

/*

Arduino va MPU6050 akselerometr va giroskop sensori qo'llanmasi Dejan, https://howtomechatronics.com */#include const int MPU = 0x68; // MPU6050 I2C manzili float AccX, AccY, AccZ; suzuvchi GyroX, GyroY, GyroZ; float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; suzuvchi rulon, qadam, yaw; float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; float elapsedTime, currentTime, previousTime; int c = 0; void setup () {Serial.begin (19200); Wire.begin (); // Wire.beginTransmission (MPU) aloqasini boshlang; // MPU6050 bilan aloqani boshlang // MPU = 0x68 Wire.write (0x6B); // 6B Wire.write (0x00) registr bilan gaplashing; // Qayta tiklash - Wire.endTransmission 6B registriga 0 qo'ying (haqiqiy); // uzatishni tugatish/* // Accelerometer sezuvchanligini sozlash - to'liq o'lchovli diapazon (standart +/- 2g) Wire.beginTransmission (MPU); Wire.write (0x1C); // ACCEL_CONFIG registri bilan gaplashing (1C hex) Wire.write (0x10); // Ro'yxatdan o'tish bitlarini 00010000 (+/- 8g to'liq o'lchovli diapazon) qilib belgilang Wire.endTransmission (rost); // Gyro sezgirligini sozlash - to'liq o'lchov diapazoni (standart +/- 250deg/s) Wire.beginTransmission (MPU); Wire.write (0x1B); // GYRO_CONFIG registri bilan gaplashing (1B olti burchakli) Wire.write (0x10); // Ro'yxatdan o'tish bitlarini 00010000 (1000deg/s to'liq o'lchovli) sifatida belgilang Wire.endTransmission (rost); kechikish (20); */ // Agar modulingiz uchun IMU xato qiymatlarini olishingiz kerak bo'lsa, ushbu funktsiyani chaqiring calc_IMU_error (); kechikish (20); } void loop () {// === Acceleromter ma'lumotlarini o'qish === // Wire.beginTransmission (MPU); Wire.write (0x3B); // 0x3B registridan boshlang (ACCEL_XOUT_H) Wire.endTransmission (noto'g'ri); Wire.requestFrom (MPU, 6, to'g'ri); // Hammasi bo'lib 6 ta registrni o'qing, har bir o'q qiymati 2 ta registrda saqlanadi // +-2g diapazoni uchun ma'lumotlar jadvaliga ko'ra, Acc38 = (Wire.read () << 8) 16384 ga bo'linishi kerak. | Wire.read ()) / 16384.0; // X o'qining qiymati AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Y o'qi qiymati AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Z -o'qi qiymati // AccAngleX akselerometr ma'lumotlaridan Roll va Pitchni hisoblash accAngleX = (atan (AccY / sqrt (pow (AccX, 2) + pow (AccZ, 2))) * 180 / PI) - 0.58; // AccErrorX ~ (0.58) Qo'shimcha ma'lumot olish uchun calc_IMU_error () maxsus funktsiyasini ko'ring accAngleY = (atan (-1 * AccX / sqrt (pow (AccY, 2) + pow (AccZ, 2))) * 180 / PI) + 1.58; // AccErrorY ~ (-1.58) // === Giroskop ma'lumotlarini o'qish === // previousTime = currentTime; // Oldingi vaqt o'qiladigan currentTime = millis () haqiqiy vaqtidan oldin saqlanadi. // Joriy vaqt haqiqiy o'qilgan vaqt elapsedTime = (currentTime - previousTime) / 1000; // soniyalarni olish uchun 1000 ga bo'linadi Wire.beginTransmission (MPU); Wire.write (0x43); // Gyro ma'lumotlarining birinchi ro'yxatga olish manzili 0x43 Wire.endTransmission (noto'g'ri); Wire.requestFrom (MPU, 6, to'g'ri); // Hammasi bo'lib 4 ta registrni o'qing, har bir o'qning qiymati 2 registrda saqlanadi GyroX = (Wire.read () << 8 | Wire.read ()) / 131.0; // 250deg/ s diapazonida biz birinchi navbatda GyroY = (Wire.read () << 8 | Wire.read ())/ 131.0; GyroZ = (Wire.read () << 8 | Wire.read ()) / 131.0; // Chiqishlarni GyroX = GyroX + 0.56 hisoblangan xato qiymatlari bilan to'g'rilang; // GyroErrorX ~ (-0.56) GyroY = GyroY - 2; // GyroErrorY ~ (2) GyroZ = GyroZ + 0.79; // GyroErrorZ ~ (-0.8) // Hozirgi vaqtda xom qiymatlar soniyalarda gradus/gradusda, shuning uchun burchakni gradusda olish uchun gondroAngleX = gyroAngleX + GyroX * elapsedTime; // deg/s * s = deg gyroAngleY = gyroAngleY + GyroY * elapsedTime; yaw = yaw + GyroZ * elapsedTime; // Qo'shimcha filtr - akseleromator va gyro burchagi qiymatlarini birlashtiradi = 0,96 * gyroAngleX + 0,04 * accAngleX; qadam = 0.96 * gyroAngleY + 0.04 * accAngleY; // Serial.print (rulon) ketma -ket monitorda qiymatlarni chop etish; Serial.print ("/"); Ketma -ket chop etish (qadam); Serial.print ("/"); Serial.println (yaw); } void calc_IMU_error () {// Biz akselerometr va gyro ma'lumotlar xatosini hisoblash uchun sozlash bo'limida bu funktsiyani chaqirishimiz mumkin. Bu erdan biz ketma -ket monitorda bosilgan yuqoridagi tenglamalarda ishlatilgan xato qiymatlarini olamiz. // E'tibor bering, biz to'g'ri qiymatlarni olish uchun O'IHni tekis joylashtirishimiz kerak, shunda biz to'g'ri qiymatlarni olamiz // Akselerometr qiymatlarini 200 marta o'qing (c <200) {Wire.beginTransmission (MPU); Wire.write (0x3B); Wire.endTransmission (noto'g'ri); Wire.requestFrom (MPU, 6, to'g'ri); AccX = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccY = (Wire.read () << 8 | Wire.read ()) / 16384.0; AccZ = (Wire.read () << 8 | Wire.read ()) / 16384.0; // Barcha o'qishlarni yig'ish 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 ++; } // Summani 200 ga bo'linib, AccErrorX = AccErrorX /200 xato qiymatini oling; AccErrorY = AccErrorY / 200; c = 0; // gyro qiymatlarini 200 marta o'qish paytida (c <200) {Wire.beginTransmission (MPU); Wire.write (0x43); Wire.endTransmission (noto'g'ri); Wire.requestFrom (MPU, 6, to'g'ri); GyroX = Wire.read () << 8 | Wire.read (); GyroY = Wire.read () << 8 | Wire.read (); GyroZ = Wire.read () << 8 | Wire.read (); // Barcha o'qishlarni yig'ing GyroErrorX = GyroErrorX + (GyroX / 131.0); GyroErrorY = GyroErrorY + (GyroY / 131.0); GyroErrorZ = GyroErrorZ + (GyroZ / 131.0); c ++; } // GyroErrorX = GyroErrorX /200 xato qiymatini olish uchun summani 200 ga bo'ling. GyroErrorY = GyroErrorY / 200; GyroErrorZ = GyroErrorZ / 200; // Serial Monitor Serial.print ("AccErrorX:") xato qiymatlarini chop eting; 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); } ------------------------------------------------- ---------------------------------------------- Natijalar:-X = Y = Z = --------------------------------------------- ----------------------------------------------- Muhim eslatma: -----------------

Loop bo'limida biz akselerometr ma'lumotlarini o'qishdan boshlaymiz. Har bir o'q uchun ma'lumotlar 2 bayt yoki registrda saqlanadi va biz bu registrlarning manzillarini sensorning ma'lumotlar sahifasidan ko'rishimiz mumkin.

Hammasini o'qish uchun biz birinchi registrdan boshlaymiz va RequiestFrom () funktsiyasidan foydalanib, X, Y va Z o'qlari uchun 6 ta registrni o'qishni so'raymiz. Keyin biz har bir registrdagi ma'lumotlarni o'qiymiz va natijalar ikkita to'ldiruvchi bo'lgani uchun ularni to'g'ri qiymatlarni olish uchun mos ravishda birlashtiramiz.

5 -qadam: Burilish burchagini tushunish

Akselerometr

Erning tortishish kuchi doimo Yer markaziga ishora qiladigan doimiy tezlanishdir.

Agar akselerometr tortishish kuchi bilan parallel bo'lsa, o'lchangan tezlanish 1G bo'ladi, akselerometr tortishish kuchi bilan perpendikulyar bo'lsa, u 0G ni o'lchaydi.

Burilish burchagini bu tenglama yordamida o'lchangan tezlanishdan hisoblash mumkin:

θ = sin-1 (O'lchangan tezlashuv / tortishish tezligi)

GyroGyro (tezlik sensori) burchak tezligini o'lchash uchun ishlatiladi.

Robotning egilish burchagini olish uchun biz quyidagi tenglamada ko'rsatilgandek, gyrodan ma'lumotlarni birlashtirishimiz kerak:

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

Gyro va akselerometr sensori birikmasi Ham gyro, ham akselerometrning xususiyatlarini o'rganib chiqib, ularning kuchli va zaif tomonlari borligini bilamiz. Akselerometr ma'lumotlari bo'yicha hisoblangan burilish burchagi sekin javob berish vaqtiga ega, gyro ma'lumotlarining egilish burchagi esa ma'lum vaqt davomida siljishga uchraydi. Boshqacha qilib aytganda, akselerometr ma'lumotlari uzoq muddatli, gyro ma'lumotlari esa qisqa muddatli uchun foydali deb aytishimiz mumkin.

Yaxshi tushunish uchun havola: bu erni bosing