Diff: main.cpp

Revision:

2:07e29b6d29da

Parent:

1:ec0a08108442

--- a/main.cpp	Sat Nov 23 16:47:59 2013 +0000
+++ b/main.cpp	Wed Mar 18 22:21:23 2015 +0000
@@ -48,6 +48,7 @@
 #include "I2Cdev.h"
 
 #include "MPU6050_6Axis_MotionApps20.h"
+#include "MPU6051_6Axis_MotionApps20.h"
 //#include "MPU6050.h" // not necessary if using MotionApps include file
 
 // class default I2C address is 0x68
@@ -56,6 +57,7 @@
 // AD0 high = 0x69
 
 MPU6050 mpu;
+MPU6051 mpu2;
 
 /* =========================================================================
    NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
@@ -77,10 +79,10 @@
 
 const float M_PI = 3.14159265;
 
-// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
+//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
+#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.
@@ -93,7 +95,7 @@
 // from the FIFO. Note this also requires gravity vector calculations.
 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_YAWPITCHROLL
+#define OUTPUT_READABLE_YAWPITCHROLL
 
 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
 // components with gravity removed. This acceleration reference frame is
@@ -118,7 +120,14 @@
 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
+uint8_t fifoBuffer[128]; // FIFO storage buffer
+
+bool dmpReady2 = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus2;   // holds actual interrupt status byte from MPU
+uint8_t devStatus2;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize2;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount2;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer2[128]; // FIFO storage buffer
 
 // orientation/motion vars
 Quaternion q;           // [w, x, y, z]         quaternion container
@@ -129,13 +138,18 @@
 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' };
 
-DigitalOut led1(LED1);
-InterruptIn checkpin(p29);
-Serial pc(USBTX, USBRX);
-
+DigitalOut led1(PTE3);
+InterruptIn checkpin(PTD7);  //PTD7, PTD5
+InterruptIn checkpin2(PTD5);
+Serial pc(PTA2,PTA1);
+DigitalOut btSwitch(PTE30); 
+DigitalOut imuSwitch(PTD6); //PTD6, PTD4 
+DigitalOut imu2Switch(PTD4);
+//int counter = 0;
 // ================================================================
 // ===               INTERRUPT DETECTION ROUTINE                ===
 // ================================================================
@@ -144,14 +158,26 @@
 void dmpDataReady() {
     mpuInterrupt = true;
 }
+volatile bool mpuInterrupt2 = false;     // indicates whether MPU interrupt pin has gone high
+void dmpDataReady2() {
+    mpuInterrupt2 = true;
+}
 
 void setup();
 void loop();
-
+void loop2();
+Timer t;
 int main() {
     setup();
+    t.start();
+    pc.baud(115200);
     while(1) {
+      //  pc.printf("Loopy loop\n\r");
+      //pc.printf("%f\t",t.read());
         loop();
+        //pc.printf("\t");
+        loop2();
+        //pc.printf("\n\r");
     }
 }
 
@@ -165,7 +191,25 @@
     // 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.
-
+btSwitch = 1;
+imuSwitch = 1;
+imu2Switch = 1;
+wait_ms(200);
+while(!mpu.testConnection()){
+    imuSwitch = 0;
+    wait_ms(100);
+    imuSwitch = 1;
+    wait_ms(200);
+    led1 = !led1;
+}
+while(!mpu2.testConnection()){
+    imu2Switch = 0;
+    wait_ms(100);
+    imu2Switch = 1;
+    wait_ms(200);
+    led1 = !led1;
+}
+pc.baud(115200);
     // initialize device
     pc.printf("Initializing I2C devices...\r\n");
     mpu.initialize();
@@ -175,12 +219,6 @@
     if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
     else pc.printf("MPU6050 connection failed\r\n");
     
-    // 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
     pc.printf("Initializing DMP...\r\n");
     devStatus = mpu.dmpInitialize();
@@ -213,7 +251,47 @@
         pc.printf("%d", devStatus);
         pc.printf(")\r\n");
     }
+ // initialize device
+    pc.printf("Initializing I2C devices...\r\n");
+    mpu2.initialize();
 
+    // verify connection
+    pc.printf("Testing device connections...\r\n");
+    if (mpu2.testConnection()) pc.printf("MPU6050 connection successful\r\n");
+    else pc.printf("MPU6050 connection failed\r\n");
+    
+    // load and configure the DMP
+    pc.printf("Initializing DMP...\r\n");
+    devStatus2 = mpu2.dmpInitialize();
+    
+    // make sure it worked (returns 0 if so)
+    if (devStatus2 == 0) {
+        // turn on the DMP, now that it's ready
+        pc.printf("Enabling DMP...\r\n");
+        mpu2.setDMPEnabled(true);
+
+        // enable Arduino interrupt detection
+        pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
+        checkpin2.rise(&dmpDataReady2);
+        
+        mpuIntStatus2 = mpu2.getIntStatus();
+
+        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        pc.printf("DMP ready! Waiting for first interrupt...\r\n");
+        dmpReady2 = true;
+
+        // get expected DMP packet size for later comparison
+        packetSize2 = mpu2.dmpGetFIFOPacketSize();
+    } else {
+        // ERROR!
+        // 1 = initial memory load failed
+        // 2 = DMP configuration updates failed
+        // (if it's going to break, usually the code will be 1)
+        
+        pc.printf("DDMP Initialization failed (code ");
+        pc.printf("%d", devStatus2);
+        pc.printf(")\r\n");
+    }
 }
 
 
@@ -238,6 +316,8 @@
         // .
         // .
         // .
+        //led1 = !led1;
+        //wait_ms(100);
     }
 
     // reset interrupt flag and get INT_STATUS byte
@@ -265,6 +345,8 @@
         // (this lets us immediately read more without waiting for an interrupt)
         fifoCount -= packetSize;
 
+                    
+            printf("\n\r%.4f\t",t.read());
         #ifdef OUTPUT_READABLE_QUATERNION
             // display quaternion values in easy matrix form: w x y z
             mpu.dmpGetQuaternion(&q, fifoBuffer);
@@ -272,7 +354,7 @@
             printf("%f\t", q.w);
             printf("%f\t", q.x);
             printf("%f\t", q.y);
-            printf("%f\t\r\n", q.z);
+            printf("%f\t", q.z);
         #endif
 
         #ifdef OUTPUT_READABLE_EULER
@@ -282,7 +364,7 @@
             printf("euler\t");
             printf("%f\t", euler[0] * 180/M_PI);
             printf("%f\t", euler[1] * 180/M_PI);
-            printf("%f\t\r\n", euler[2] * 180/M_PI);
+            printf("%f\t\", euler[2] * 180/M_PI);
         #endif
 
         #ifdef OUTPUT_READABLE_YAWPITCHROLL
@@ -291,9 +373,9 @@
             mpu.dmpGetGravity(&gravity, &q);
             mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
             printf("ypr\t");
-            printf("%f\t", ypr[0] * 180/M_PI);
-            printf("%f\t", ypr[1] * 180/M_PI);
-            printf("%f\t\r\n", ypr[2] * 180/M_PI);
+            printf("%.4f\t", ypr[0] * 180/M_PI);
+            printf("%.4f\t", ypr[1] * 180/M_PI);
+            printf("%.4f\t", ypr[2] * 180/M_PI);
         #endif
 
         #ifdef OUTPUT_READABLE_REALACCEL
@@ -303,9 +385,9 @@
             mpu.dmpGetGravity(&gravity, &q);
             mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
             printf("areal\t");
-            printf("%f\t", aaReal.x);
-            printf("%f\t", aaReal.y);
-            printf("%f\t\r\n", aaReal.z);
+            printf("%f.4\t", aaReal.x);
+            printf("%f.4\t", aaReal.y);
+            printf("%f.4\t", aaReal.z);
         #endif
 
         #ifdef OUTPUT_READABLE_WORLDACCEL
@@ -316,9 +398,9 @@
             mpu.dmpGetGravity(&gravity, &q);
             mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
             printf("aworld\t");
-            printf("%f\t", aaWorld.x);
-            printf("%f\t", aaWorld.y);
-            printf("%f\t\r\n", aaWorld.z);
+            printf("%f.4\t", aaWorld.x);
+            printf("%f.4\t", aaWorld.y);
+            printf("%f.4\t", aaWorld.z);
         #endif
     
         #ifdef OUTPUT_TEAPOT
@@ -338,6 +420,129 @@
         #endif
 
         // blink LED to indicate activity
-        led1 = !led1;
+        //led1 = !led1;
+    }
+}
+
+
+void loop2() {
+    // if programming failed, don't try to do anything
+    if (!dmpReady2) return;
+
+    // wait for MPU interrupt or extra packet(s) available
+    while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
+        // 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
+        // .
+        // .
+        // .
+        //led1 = !led1;
+        //wait_ms(100);
+    }
+
+    // reset interrupt flag and get INT_STATUS byte
+    mpuInterrupt2 = false;
+    mpuIntStatus2 = mpu2.getIntStatus();
+
+    // get current FIFO count
+    fifoCount2 = mpu2.getFIFOCount();
+
+    // check for overflow (this should never happen unless our code is too inefficient)
+    if ((mpuIntStatus2 & 0x10) || fifoCount2 == 1024) {
+        // reset so we can continue cleanly
+        mpu2.resetFIFO();
+        //Serial.println(F("FIFO overflow!"));
+
+    // otherwise, check for DMP data ready interrupt (this should happen frequently)
+    } else if (mpuIntStatus2 & 0x02) {
+        // wait for correct available data length, should be a VERY short wait
+        while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();
+
+        // read a packet from FIFO
+        mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
+        
+        // track FIFO count here in case there is > 1 packet available
+        // (this lets us immediately read more without waiting for an interrupt)
+        fifoCount2 -= packetSize2;
+
+        #ifdef OUTPUT_READABLE_QUATERNION
+            // display quaternion values in easy matrix form: w x y z
+            mpu2.dmpGetQuaternion(&q, fifoBuffer2);
+            printf("quat\t");
+            printf("%.4f\t", q.w);
+            printf("%.4f\t", q.x);
+            printf("%.4f\t", q.y);
+            printf("%.4f\t", q.z);
+        #endif
+
+        #ifdef OUTPUT_READABLE_EULER
+            // display Euler angles in degrees
+            mpu2.dmpGetQuaternion(&q, fifoBuffer2);
+            mpu2.dmpGetEuler(euler, &q);
+            printf("euler\t");
+            printf("%.4f\t", euler2[0] * 180/M_PI);
+            printf("%.4f\t", euler2[1] * 180/M_PI);
+            printf("%.4f\t", euler2[2] * 180/M_PI);
+        #endif
+
+        #ifdef OUTPUT_READABLE_YAWPITCHROLL
+            // display Euler angles in degrees
+            mpu2.dmpGetQuaternion(&q, fifoBuffer2);
+            mpu2.dmpGetGravity(&gravity, &q);
+            mpu2.dmpGetYawPitchRoll(ypr, &q, &gravity);
+            printf("ypr\t");
+            printf("%.4f\t", ypr[0] * 180/M_PI);
+            printf("%.4f\t", ypr[1] * 180/M_PI);
+            printf("%.4f\t", 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);
+            printf("areal\t");
+            printf("%.4f\t", aaReal.x);
+            printf("%.4f\t", aaReal.y);
+            printf("%.4f\t\r\n", 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.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
+            printf("aworld\t");
+            printf("%f\t", aaWorld.x);
+            printf("%f\t", aaWorld.y);
+            printf("%f\t\r\n", 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];
+            for (int i = 0; i < 14; ++i) {
+                pc.send(teapotPacket[i]);
+            }
+            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
+        #endif*/
+            printf("\r\n");
+        // blink LED to indicate activity
+        //led1 = !led1;
     }
 }
\ No newline at end of file