Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include "I2Cdev.h" // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
- #include "MPU6050_6Axis_MotionApps20.h"
- #include "MPU6050.h" // not necessary if using MotionApps include file
- #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
- #include "Wire.h"
- #endif
- MPU6050 mpu;
- //MPU6050 mpu(0x69); // <-- use for AD0 high
- /* ======================
- | Pin | Connection |
- ======================
- | A4 | (SDA) |
- | A5 | (SCL) |
- | 3.3V | VCC |
- | GND | GND |
- | 2 | INIT |
- ====================== */
- // Uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
- // quaternion components in a [w, x, y, z] format (not best for parsing
- // on a remote host such as Processing or something though)
- // #define OUTPUT_READABLE_QUATERNION
- // Uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
- // (in degrees) calculated from the quaternions coming from the FIFO.
- // Note that Euler angles suffer from gimbal lock - http://en.wikipedia.org/wiki/Gimbal_lock
- // #define OUTPUT_READABLE_EULER
- // Uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
- // pitch/roll angles (in degrees) calculated from the quaternions coming
- // from the FIFO. Note this also requires gravity vector calculations.
- // Also note that yaw/pitch/roll angles suffer from gimbal lock - http://en.wikipedia.org/wiki/Gimbal_lock
- #define OUTPUT_READABLE_YAWPITCHROLL
- // Uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
- // components with gravity removed. This acceleration reference frame is
- // not compensated for orientation, so +X is always +X according to the
- // sensor, just without the effects of gravity. If you want acceleration
- // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
- // #define OUTPUT_READABLE_REALACCEL
- // Uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
- // components with gravity removed and adjusted for the world frame of
- // reference (yaw is relative to initial orientation, since no magnetometer
- // is present in this case). Could be quite handy in some cases.
- // #define OUTPUT_READABLE_WORLDACCEL
- // Uncomment "OUTPUT_TEAPOT" if you want output that matches the
- // format used for the InvenSense teapot demo
- // #define OUTPUT_TEAPOT
- #define INTERRUPT_PIN 2 // INIT for MPU6050
- #define LED_PIN 13 // Arduino is 13 for Indication
- bool blinkState = false;
- // MPU Control / Status Vars
- bool dmpReady = false; // Set true if DMP init was successful
- uint8_t mpuIntStatus; // Holds actual interrupt status byte from MPU
- uint8_t devStatus; // Return status after each device operation (0 = success, !0 = error)
- uint16_t packetSize; // Expected DMP packet size (default is 42 bytes)
- uint16_t fifoCount; // Count of all bytes currently in FIFO
- uint8_t fifoBuffer[64]; // FIFO storage buffer
- // Orientation / Motion Vars
- Quaternion q; // [w, x, y, z] Quaternion Container
- VectorInt16 aa; // [x, y, z] Accelorometer Sensor Measurements
- VectorInt16 aaReal; // [x, y, z] Gravity - Free Accelorometer Sensor Measurements
- VectorInt16 aaWorld; // [x, y, z] World - Frame Accelorometer Sensor Measurements
- VectorFloat gravity; // [x, y, z] Gravity Vector
- float euler[3]; // [psi, theta, phi] Euler angle container
- float ypr[3]; // [yaw, pitch, roll] Yaw / Pitch / Roll Container and Gravity Vector
- // Packet Structure for InvenSense teapot demo
- uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
- // ================================================================
- // === INTERRUPT DETECTION ROUTINE ===
- // ================================================================
- volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
- void dmpDataReady()
- {
- mpuInterrupt = true;
- }
- // ================================================================
- // === INITIAL SETUP ===
- // ================================================================
- void setup()
- {
- // Join I2C bus (I2Cdev Library doesn't do this automatically)
- #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
- Wire.begin();
- Wire.setClock(400000);
- #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
- Fastwire::setup(400, true);
- #endif
- // Initialize Serial Communication (115200 chosen because it is required for Teapot Demo output, but it's really up to you depending on your project)
- Serial.begin(115200);
- while (!Serial);
- // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
- // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
- // the baud timing being too misaligned with processor ticks. You must use
- // 38400 or slower in these cases, or use some kind of external separate
- // crystal solution for the UART timer.
- // Initialize Device
- Serial.println(F("Initializing I2C devices..."));
- mpu.initialize();
- pinMode(INTERRUPT_PIN, INPUT);
- // Verify Connection
- Serial.println(F("Testing device connections..."));
- Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
- // Wait for ready
- Serial.println(F("\nSend any character to begin DMP programming and demo: "));
- while (Serial.available() && Serial.read()); // Empty Buffer
- while (!Serial.available()); // Wait for Data
- while (Serial.available() && Serial.read()); // Empty Buffer Again
- // Load and Configure the DMP
- Serial.println(F("Initializing DMP..."));
- devStatus = mpu.dmpInitialize();
- // Supply your own Gyro Offsets here, scaled for Minimum Sensitivity
- mpu.setXGyroOffset(220);
- mpu.setYGyroOffset(76);
- mpu.setZGyroOffset(-85);
- mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
- // make sure it worked (returns 0 if so)
- if (devStatus == 0)
- {
- Serial.println(F(" Enabling DMP... ")); // turn on the DMP, now that it's ready
- mpu.setDMPEnabled(true);
- Serial.println(F(" Enabling interrupt detection (Arduino external interrupt 0)... ")); // Enable Arduino interrupt detection
- attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
- mpuIntStatus = mpu.getIntStatus();
- Serial.println(F(" DMP ready! Waiting for first interrupt... ")); // Set our DMP Ready flag so the main loop() function knows it's okay to use it
- dmpReady = true;
- packetSize = mpu.dmpGetFIFOPacketSize(); // get expected DMP packet size for later comparison
- }
- else
- {
- // ERROR!
- // 1 = initial memory load failed
- // 2 = DMP configuration updates failed
- // (if it's going to break, usually the code will be 1)
- Serial.print(F(" DMP Initialization failed (code : "));
- Serial.print(devStatus);
- Serial.println(F(")"));
- }
- pinMode(LED_PIN, OUTPUT); // configure LED for output
- }
- // ================================================================
- // === MAIN PROGRAM LOOP ===
- // ================================================================
- void loop()
- {
- if (!dmpReady) // if programming failed, don't try to do anything
- return;
- while (!mpuInterrupt && fifoCount < packetSize) // wait for MPU interrupt or extra packet(s) available
- {
- // other program behavior stuff here
- // if you are really paranoid you can frequently test in between other
- // stuff to see if mpuInterrupt is true, and if so, "break;" from the
- // while() loop to immediately process the MPU data
- }
- mpuInterrupt = false; // reset interrupt flag and get INT_STATUS byte
- mpuIntStatus = mpu.getIntStatus();
- fifoCount = mpu.getFIFOCount(); // get current FIFO count
- if ((mpuIntStatus & 0x10) || fifoCount == 1024) // Check for overflow (this should never happen unless our code is too inefficient)
- {
- mpu.resetFIFO(); // reset so we can continue cleanly
- Serial.println(F("FIFO overflow!"));
- }
- else if (mpuIntStatus & 0x02) // Otherwise, check for DMP data ready interrupt (this should happen frequently)
- {
- while (fifoCount < packetSize) // Wait for correct available data length, should be a VERY short wait
- fifoCount = mpu.getFIFOCount();
- mpu.getFIFOBytes(fifoBuffer, packetSize); // Read a packet from FIFO
- fifoCount -= packetSize; // Track FIFO count here in case there is > 1 packet available (this lets us immediately read more without waiting for an interrupt)
- #ifdef OUTPUT_READABLE_QUATERNION // Display quaternion values in easy matrix form: 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 // Display Euler angles in degrees
- 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 // Display Euler angles in degrees
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
- 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 // Display Real Acceleration, Adjusted to Remove Gravity
- mpu.dmpGetQuaternion(&q, fifoBuffer);
- mpu.dmpGetAccel(&aa, fifoBuffer);
- mpu.dmpGetGravity(&gravity, &q);
- mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
- Serial.print("areal\t");
- Serial.print(aaReal.x);
- Serial.print("\t");
- Serial.print(aaReal.y);
- Serial.print("\t");
- Serial.println(aaReal.z);
- #endif
- #ifdef OUTPUT_READABLE_WORLDACCEL // Display Initial World-Frame Acceleration, Adjusted to Remove Gravity and Rotated based on known Orientation from 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);
- Serial.print("\t");
- Serial.print(aaWorld.y);
- Serial.print("\t");
- Serial.println(aaWorld.z);
- #endif
- #ifdef OUTPUT_TEAPOT // Display Quaternion values in InvenSense Teapot demo format:
- 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, Loops at 0xFF on purpose
- #endif
- blinkState = !blinkState; // Blink LED to Indicate Activity
- digitalWrite(LED_PIN, blinkState);
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement