Aan de slag met IMU (6 DOF) bewegingssensor

Code

• Arduino-code voor MPU-sensor

Arduino-code voor MPU-sensorArduino

*/// I2Cdev en MPU6050 moeten als bibliotheken worden geïnstalleerd, anders moeten de .cpp/.h-bestanden// voor beide klassen in het include-pad van uw project staan#include "I2Cdev.h"#include "MPU6050_6Axis_MotionApps20 .h"//#include "MPU6050.h" // niet nodig bij gebruik van MotionApps include file// Arduino Wire-bibliotheek is vereist als I2Cdev I2CDEV_ARDUINO_WIRE implementatie// wordt gebruikt in I2Cdev.h#if I2CDEV_IMPLEMENTATION ==I2CDEV_ARDUINO_WIRE #include "Wire .h"#endif// class standaard I2C-adres is 0x68// specifieke I2C-adressen kunnen hier als parameter worden doorgegeven// AD0 laag =0x68 (standaard voor SparkFun breakout en InvenSense evaluatiebord)// AD0 hoog =0x69MPU6050 mpu;/ /MPU6050 mpu (0x69); // <-- gebruik voor AD0 hoog/* =========================================================================OPMERKING:Naast aansluiting 3.3v, GND, SDA en SCL, deze schets hangt af van het feit dat de INT-pin van de MPU-6050 is aangesloten op de externe interrupt #0-pin van de Arduino. Op de Arduino Uno en Mega 2560 is dit digitale I/O pin 2. * =========================================================================*//* =========================================================================OPMERKING:Arduino v1.0.1 met het Leonardo-bord genereert een compileerfout bij gebruik van Serial.write (buf, len). De output van de theepot gebruikt deze methode. De oplossing vereist een aanpassing aan het Arduino USBAPI.h-bestand, wat gelukkig eenvoudig, maar vervelend is. Dit wordt opgelost in de volgende IDE-release. Zie deze links voor meer informatie:http://arduino.cc/forum/index.php/topic,109987.0.html http://code.google.com/p/arduino/issues/detail?id=958 * =========================================================================*/// verwijder commentaar "OUTPUT_READABLE_QUATERNION" als u de werkelijke// quaternioncomponenten in een [w, x, y, z] formaat (niet het beste voor het ontleden// op een externe host zoals Processing of iets dergelijks)//#define OUTPUT_READABLE_QUATERNION// uncomment "OUTPUT_READABLE_EULER" als u Euler-hoeken// (in graden) wilt zien berekend op basis van de komende quaternionen van de FIFO.// Merk op dat Euler-hoeken last hebben van gimbal-lock (voor meer info, zie// http://en.wikipedia.org/wiki/Gimbal_lock)//#define OUTPUT_READABLE_EULER// uncomment "OUTPUT_READABLE_YAWPITCHROLL" als je wilt om de yaw/// pitch/roll-hoeken (in graden) te zien berekend op basis van de quaternionen die komen// van de FIFO. Merk op dat hiervoor ook zwaartekrachtvectorberekeningen nodig zijn.// Merk ook op dat yaw/pitch/roll-hoeken last hebben van gimbal-lock (voor// meer info, zie:http://en.wikipedia.org/wiki/Gimbal_lock)#define OUTPUT_READABLE_YAWPITCHROLL/ / uncomment "OUTPUT_READABLE_REALACCEL" als u acceleratie// componenten met zwaartekracht verwijderd wilt zien. Dit versnellingsreferentieframe is// niet gecompenseerd voor oriëntatie, dus +X is altijd +X volgens de// sensor, alleen zonder de effecten van de zwaartekracht. Als u versnelling// gecompenseerde oriëntatie wilt, gebruikt u in plaats daarvan OUTPUT_READABLE_WORLDACCEL.//#define OUTPUT_READABLE_REALACCEL// uncomment "OUTPUT_READABLE_WORLDACCEL" als u versnelling// componenten met zwaartekracht verwijderd en aangepast voor het wereldframe van// referentie (yaw is ten opzichte van de initiële oriëntatie, aangezien in dit geval geen magnetometer// aanwezig is). Kan in sommige gevallen best handig zijn.//#define OUTPUT_READABLE_WORLDACCEL// verwijder het commentaar bij "OUTPUT_TEAPOT" als u uitvoer wilt die overeenkomt met het//-formaat dat wordt gebruikt voor de InvenSense-theepotdemo//#define OUTPUT_TEAPOT#define LED_PIN 13 // (Arduino is 13 , Teensy is 11, Teensy++ is 6) bool blinkState =false;// MPU-besturing/status varsbool dmpReady =false; // stel waar in als DMP init succesvol wasuint8_t mpuIntStatus; // bevat actuele interruptstatusbyte van MPUuint8_t devStatus; // retourstatus na elke apparaatbewerking (0 =geslaagd, !0 =fout) uint16_t packetSize; // verwachte DMP-pakketgrootte (standaard is 42 bytes) uint16_t fifoCount; // telling van alle bytes die momenteel in FIFOuint8_t fifoBuffer [64]; // FIFO-opslagbuffer// oriëntatie/beweging varsQuaternion q; // [w, x, y, z] quaternion containerVectorInt16 aa; // [x, y, z] accel sensor metingenVectorInt16 aaReal; // [x, y, z] zwaartekracht-vrije accel sensor metingen VectorInt16 aaWorld; // [x, y, z] wereld-frame accel sensor metingen VectorFloat zwaartekracht; // [x, y, z] zwaartekracht vectorfloat euler [3]; // [psi, theta, phi] Euler hoek containerfloat ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container en zwaartekracht vector// pakketstructuur voor InvenSense theepot demouint8_t teapotPacket[14] ={ '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };// ================================================================// ===DETECTIEROUTINE ONDERBREKEN ===// ================================================================vluchtige bool mpuInterrupt =false; // geeft aan of de MPU-interrupt-pin highvoid is geworden dmpDataReady() {mpuInterrupt =true;}// ================================================================// ===EERSTE INSTELLING ===// ================================================================void setup() {// join I2C bus (I2Cdev bibliotheek doet dit niet automatisch) #if I2CDEV_IMPLEMENTATION ==I2CDEV_ARDUINO_WIRE Wire.begin(); TWBR =24; // 400 kHz I2C-klok (200 kHz als CPU 8 MHz is) #elif I2CDEV_IMPLEMENTATION ==I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif // initialiseer seriële communicatie // (115200 gekozen omdat het vereist is voor uitvoer van theepotdemo's, maar het is echt aan jou, afhankelijk van je project) Serial.begin(115200); terwijl (!Serial); // wacht op Leonardo-telling, anderen gaan onmiddellijk verder // OPMERKING:hostprocessors van 8 MHz of langzamer, zoals de Teensy @ 3.3v of Ardunio // Pro Mini die op 3.3v draait, kunnen deze baudsnelheid niet betrouwbaar aan vanwege // de baudtiming te slecht uitgelijnd met processorticks. U moet in deze gevallen // 38400 of langzamer gebruiken, of een externe aparte // kristaloplossing gebruiken voor de UART-timer. // initialiseer apparaat Serial.println (F ("I2C-apparaten initialiseren ...")); mpu.initialiseren(); // verifieer verbinding Serial.println (F ("Testapparaatverbindingen...")); Serial.println(mpu.testConnection() ? F("MPU6050 verbinding succesvol") :F("MPU6050 verbinding mislukt")); // wacht op gereed Serial.println(F("\nStuur een willekeurig teken om de DMP-programmering en demo te starten:")); while (Serial.available() &&Serial.read()); // lege buffer while (!Serial.available()); // wacht op gegevens terwijl (Serial.available() &&Serial.read()); // lege buffer opnieuw // laad en configureer de DMP Serial.println(F("Initializing DMP...")); devStatus =mpu.dmpInitialize(); // geef hier uw eigen gyro-offsets op, geschaald voor minimale gevoeligheid mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset (1788); // 1688 fabrieksinstelling voor mijn testchip // zorg ervoor dat het werkte (retourneert 0 als dat zo is) if (devStatus ==0) { // zet de DMP aan, nu deze klaar is Serial.println(F ("DMP inschakelen. ..")); mpu.setDMPEnabled(true); // Arduino-interruptdetectie inschakelen Serial.println (F ("Interruptdetectie inschakelen (Arduino externe interrupt 0) ...")); attachInterrupt(0, dmpDataReady, RISING); mpuIntStatus =mpu.getIntStatus(); // stel onze DMP Ready-vlag zo in dat de hoofdlus()-functie weet dat het goed is om deze te gebruiken Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady =waar; // haal de verwachte DMP-pakketgrootte op voor latere vergelijking packetSize =mpu.dmpGetFIFOPacketSize(); } anders { // FOUT! // 1 =initiële geheugenbelasting mislukt // 2 =DMP-configuratie-updates mislukt // (als het kapot gaat, is de code meestal 1) Serial.print(F ("DMP-initialisatie mislukt (code")); Serial. print(devStatus);Serial.println(F(")")); } // configureer LED voor output pinMode (LED_PIN, OUTPUT);}// ================================================================// ===HOOFDPROGRAMMA LUS ===// ================================================================void loop() {// als het programmeren is mislukt, probeer dan niets te doen als (!dmpReady) terugkeert; // wacht op MPU-interrupt of extra pakket(ten) beschikbaar terwijl (!mpuInterrupt &&fifoCount  1 pakket beschikbaar is // (hierdoor kunnen we meteen meer lezen zonder te wachten op een onderbreking) fifoCount -=packetSize; #ifdef OUTPUT_READABLE_QUATERNION // toon quaternion-waarden in eenvoudige matrixvorm:w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); Serial.print("quat\t"); Serial.print(q.w); Serieel.print("\t"); Serial.print(q.x); Serieel.print("\t"); Serial.print(q.y); Serieel.print("\t"); Serieel.println(q.z); #endif #ifdef OUTPUT_READABLE_EULER // toon Euler-hoeken in graden mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetEuler(euler, &q); Serial.print("euler\t"); Serial.print(euler[0] * 180/M_PI); Serieel.print("\t"); Serial.print(euler[1] * 180/M_PI); Serieel.print("\t"); Serial.println(euler[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // toon Euler-hoeken in graden mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); Serial.print("ypr\t"); Serial.print(ypr[0] * 180/M_PI); Serieel.print("\t"); Serial.print(ypr[1] * 180/M_PI); Serieel.print("\t"); Serial.println(ypr[2] * 180/M_PI); #endif #ifdef OUTPUT_READABLE_REALACCEL // echte versnelling weergeven, aangepast om zwaartekracht te verwijderen mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); Serial.print("areaal\t"); Serial.print(aaReal.x); Serieel.print("\t"); Serial.print(aaReal.y); Serieel.print("\t"); Serial.println(aaReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // geeft initiële wereldframeversnelling weer, aangepast om zwaartekracht te verwijderen // en geroteerd op basis van bekende oriëntatie van quaternion mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); Serial.print("aworld\t"); Serial.print(aaWorld.x); Serieel.print("\t"); Serial.print(aaWorld.y); Serieel.print("\t"); Serial.println(aaWorld.z); #endif #ifdef OUTPUT_TEAPOT // toon quaternion-waarden in InvenSense Teapot-demo-indeling:teapotPacket[2] =fifoBuffer[0]; theepotPacket[3] =fifoBuffer[1]; theepotPacket[4] =fifoBuffer[4]; theepotPacket[5] =fifoBuffer[5]; theepotPacket[6] =fifoBuffer[8]; theepotPacket[7] =fifoBuffer[9]; theepotPacket[8] =fifoBuffer[12]; theepotPacket[9] =fifoBuffer[13]; Serial.write (theepotpakket, 14); theepotPakket[11]++; // packetCount, lussen expres op 0xFF #endif // knipperende LED om activiteit aan te geven blinkState =!blinkState; digitalWrite (LED_PIN, blinkState); }}

Schema's