Memulai sensor gerak IMU (6 DOF)

Kode

• Kode Arduino untuk sensor MPU

Kode Arduino untuk sensor MPUArduino

*/// I2Cdev dan MPU6050 harus diinstal sebagai perpustakaan, atau file .cpp/.h// untuk kedua kelas harus berada di jalur sertakan proyek Anda#include "I2Cdev.h"#include "MPU6050_6Axis_MotionApps20 .h"//#include "MPU6050.h" // tidak diperlukan jika menggunakan MotionApps include file// Perpustakaan Arduino Wire diperlukan jika implementasi I2Cdev I2CDEV_ARDUINO_WIRE// digunakan dalam I2Cdev.h#jika I2CDEV_IMPLEMENTATION ==I2CDEV_ARDUINO_WIRE #include "Wire .h"#endif// alamat I2C default kelas adalah 0x68// alamat I2C tertentu dapat diteruskan sebagai parameter di sini// AD0 rendah =0x68 (default untuk breakout SparkFun dan papan evaluasi InvenSense)// AD0 tinggi =0x69MPU6050 mpu;/ /MPU6050 mpu(0x69); // <-- gunakan untuk AD0 tinggi/* =========================================================================CATATAN:Selain koneksi 3.3v, GND, SDA, dan SCL, sketsa ini tergantung pada pin INT MPU-6050 yang terhubung ke pin interupsi eksternal #0 Arduino. Pada Arduino Uno dan Mega 2560, ini adalah digital I/O pin 2. * =======================================================================*//* =========================================================================CATATAN:Arduino v1.0.1 dengan papan Leonardo menghasilkan kesalahan kompilasi saat menggunakan Serial.write(buf, len). Output Teapot menggunakan metode ini. Solusinya memerlukan modifikasi pada file Arduino USBAPI.h, yang untungnya sederhana, tetapi mengganggu. Ini akan diperbaiki dalam rilis IDE berikutnya. Untuk info lebih lanjut, lihat tautan ini:http://arduino.cc/forum/index.php/topic,109987.0.html http://code.google.com/p/arduino/issues/detail?id=958 * =========================================================================*/// batalkan komentar "OUTPUT_READABLE_QUATERNION" jika Anda ingin melihat komponen quaternion // aktual dalam [w, x, y, z] (bukan yang terbaik untuk penguraian// pada host jarak jauh seperti Processing atau semacamnya)//#define OUTPUT_READABLE_QUATERNION// batalkan komentar "OUTPUT_READABLE_EULER" jika Anda ingin melihat sudut Euler// (dalam derajat) dihitung dari quaternions yang datang dari FIFO.// Perhatikan bahwa sudut Euler menderita kunci gimbal (untuk info lebih lanjut, lihat// http://en.wikipedia.org/wiki/Gimbal_lock)//#define OUTPUT_READABLE_EULER// batalkan komentar "OUTPUT_READABLE_YAWPITCHROLL" jika Anda mau untuk melihat sudut yaw/// pitch/roll (dalam derajat) yang dihitung dari quaternions yang datang// dari FIFO. Perhatikan ini juga memerlukan perhitungan vektor gravitasi.// Perhatikan juga bahwa sudut yaw/pitch/roll menderita kunci gimbal (untuk// info lebih lanjut, lihat:http://en.wikipedia.org/wiki/Gimbal_lock)#define OUTPUT_READABLE_YAWPITCHROLL/ / batalkan komentar "OUTPUT_READABLE_REALACCEL" jika Anda ingin melihat akselerasi// komponen dengan gravitasi dihilangkan. Kerangka referensi percepatan ini// tidak dikompensasikan untuk orientasi, jadi +X selalu +X menurut sensor//, hanya tanpa efek gravitasi. Jika Anda ingin percepatan// kompensasi untuk orientasi, kami OUTPUT_READABLE_WORLDACCEL sebagai gantinya.//#define OUTPUT_READABLE_REALACCEL// batalkan komentar "OUTPUT_READABLE_WORLDACCEL" jika Anda ingin melihat percepatan// komponen dengan gravitasi dihilangkan dan disesuaikan dengan kerangka dunia// referensi (yaw adalah relatif terhadap orientasi awal, karena tidak ada magnetometer// dalam kasus ini). Bisa sangat berguna dalam beberapa kasus.//#define OUTPUT_READABLE_WORLDACCEL// batalkan komentar "OUTPUT_TEAPOT" jika Anda menginginkan keluaran yang cocok dengan// format yang digunakan untuk demo teko InvenSense//#define OUTPUT_TEAPOT#define LED_PIN 13 // (Arduino adalah 13 , Teensy adalah 11, Teensy++ adalah 6)bool blinkState =false;// MPU control/status varsbool dmpReady =false; // set true jika DMP init berhasil mpuIntStatus; // menyimpan byte status interupsi aktual dari MPUuint8_t devStatus; // mengembalikan status setelah setiap operasi perangkat (0 =sukses, !0 =error)uint16_t packetSize; // ukuran paket DMP yang diharapkan (default adalah 42 byte)uint16_t fifoCount; // jumlah semua byte yang saat ini ada di FIFOuint8_t fifoBuffer[64]; // buffer penyimpanan FIFO// orientasi/gerak varsQuaternion q; // [w, x, y, z] quaternion containerVectorInt16 aa; // [x, y, z] pengukuran sensor accelVectorInt16 aaReal; // [x, y, z] pengukuran sensor accel bebas gravitasiVectorInt16 aaWorld; // [x, y, z] pengukuran sensor aksel bingkai duniaVectorFloat gravity; // [x, y, z] gravitasi vektorfloat euler[3]; // [psi, theta, phi] Euler angle containerfloat ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container dan vektor gravitasi// struktur paket untuk InvenSense teapot demouint8_t teapotPacket[14] ={ '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };// ==============================================================// ===RUTIN DETEKSI GANGGUAN ===// ================================================================volatile bool mpuInterrupt =false; // menunjukkan apakah pin interupsi MPU telah menjadi highvoid dmpDataReady() { mpuInterrupt =true;}// ================================================================// ===SETUP AWAL ===// ================================================================void setup() { // bergabung dengan bus I2C (library I2Cdev tidak melakukan ini secara otomatis) #if I2CDEV_IMPLEMENTATION ==I2CDEV_ARDUINO_WIRE Wire.begin(); TWBR =24; // 400kHz I2C clock (200kHz jika CPU adalah 8MHz) #elif I2CDEV_IMPLEMENTATION ==I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // menginisialisasi komunikasi serial // (115200 dipilih karena diperlukan untuk output Teapot Demo, tapi itu // terserah Anda tergantung pada proyek Anda) Serial.begin(115200); while (!Serial); // tunggu penghitungan Leonardo, yang lain segera melanjutkan // CATATAN:8MHz atau prosesor host yang lebih lambat, seperti Teensy @ 3.3v atau Ardunio // Pro Mini yang berjalan pada 3.3v, tidak dapat menangani baud rate ini dengan andal karena // waktu baud terlalu tidak selaras dengan kutu prosesor. Anda harus menggunakan // 38400 atau lebih lambat dalam kasus ini, atau menggunakan semacam solusi kristal // eksternal terpisah untuk pengatur waktu UART. // inisialisasi perangkat Serial.println(F("Menginisialisasi perangkat I2C...")); mpu.initialize(); // verifikasi koneksi Serial.println(F("Menguji koneksi perangkat...")); Serial.println(mpu.testConnection() ? F("Koneksi MPU6050 berhasil") :F("Koneksi MPU6050 gagal")); // tunggu Serial.println(F("\nKirim karakter apa saja untuk memulai pemrograman DMP dan demo:")); while (Serial.available() &&Serial.read()); // kosongkan buffer while (!Serial.available()); // menunggu data sementara (Serial.available() &&Serial.read()); // kosongkan buffer lagi // muat dan konfigurasikan DMP Serial.println(F("Inisialisasi DMP...")); devStatus =mpu.dmpInitialize(); // berikan offset gyro Anda sendiri di sini, diskalakan untuk sensitivitas min mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); // 1688 default pabrik untuk chip pengujian saya // pastikan itu berfungsi (mengembalikan 0 jika demikian) if (devStatus ==0) { // nyalakan DMP, sekarang sudah siap Serial.println(F("Mengaktifkan DMP. ..")); mpu.setDMPEnabled(benar); // aktifkan deteksi interupsi Arduino Serial.println(F("Mengaktifkan deteksi interupsi (interupsi eksternal Arduino 0)...")); attachInterrupt(0, dmpDataReady, RISING); mpuIntStatus =mpu.getIntStatus(); // atur flag DMP Ready kita sehingga fungsi loop() utama tahu bahwa tidak apa-apa untuk menggunakannya Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpSiap =benar; // dapatkan ukuran paket DMP yang diharapkan untuk perbandingan nanti packetSize =mpu.dmpGetFIFOPacketSize(); } lain { // GALAT! // 1 =gagal memuat memori awal // 2 =Pembaruan konfigurasi DMP gagal // (jika akan rusak, biasanya kodenya adalah 1) Serial.print(F("Inisialisasi DMP gagal (kode ")); Serial. print(devStatus); Serial.println(F(")")); } // konfigurasikan LED untuk output pinMode(LED_PIN, OUTPUT);}// ================================================================// ===LOOP PROGRAM UTAMA ===// ================================================================void loop() { // jika pemrograman gagal, jangan mencoba melakukan apapun jika (!dmpReady) kembali; // menunggu interupsi MPU atau paket tambahan tersedia saat (!mpuInterrupt &&fifoCount  1 paket yang tersedia // (ini memungkinkan kita segera membaca lebih banyak tanpa menunggu interupsi) fifoCount -=packetSize; #ifdef OUTPUT_READABLE_QUATERNION // menampilkan nilai quaternion dalam bentuk matriks yang mudah:w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); Serial.print("quat\t"); Serial.print(q.w); Serial.print("\t"); Serial.print(q.x); Serial.print("\t"); Serial.print(q.y); Serial.print("\t"); Serial.println(q.z); #endif #ifdef OUTPUT_READABLE_EULER // menampilkan sudut Euler dalam derajat mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); Serial.print("euler\t"); Serial.print(euler[0] * 180/M_PI); Serial.print("\t"); Serial.print(euler[1] * 180/M_PI); Serial.print("\t"); Serial.println(euler[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // menampilkan sudut Euler dalam derajat mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravitasi, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravitasi); Serial.print("ypr\t"); Serial.print(ypr[0] * 180/M_PI); Serial.print("\t"); Serial.print(ypr[1] * 180/M_PI); Serial.print("\t"); Serial.println(ypr[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_REALACCEL // menampilkan akselerasi nyata, disesuaikan untuk menghilangkan gravitasi mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravitasi, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); Serial.print("area\t"); Serial.print(aaReal.x); Serial.print("\t"); Serial.print(aaReal.y); Serial.print("\t"); Serial.println(aaReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // menampilkan akselerasi kerangka dunia awal, disesuaikan untuk menghilangkan gravitasi // dan diputar berdasarkan orientasi yang diketahui dari quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravitasi, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); Serial.print("dunia\t"); Serial.print(aaWorld.x); Serial.print("\t"); Serial.print(aaWorld.y); Serial.print("\t"); Serial.println(aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // menampilkan nilai quaternion dalam format demo InvenSense Teapot:teapotPacket[2] =fifoBuffer[0]; teapotPacket[3] =fifoBuffer[1]; teapotPacket[4] =fifoBuffer[4]; teapotPacket[5] =fifoBuffer[5]; teapotPacket[6] =fifoBuffer[8]; teapotPacket[7] =fifoBuffer[9]; teapotPacket[8] =fifoBuffer[12]; teapotPacket[9] =fifoBuffer[13]; Serial.write(teapotPacket, 14); teapotPacket[11]++; // packetCount, loop pada 0xFF dengan sengaja #endif // LED berkedip untuk menunjukkan aktivitas blinkState =!blinkState; digitalWrite(LED_PIN, status berkedip); }}

Skema