Diff: main.cpp

Revision:

1:ec0a08108442

Parent:

0:8d2c753a96e7

Child:

2:42c4f3a7813f

--- a/main.cpp	Tue Nov 19 19:10:30 2013 +0000
+++ b/main.cpp	Sat Nov 23 16:47:59 2013 +0000
@@ -54,6 +54,7 @@
 // specific I2C addresses may be passed as a parameter here
 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
 // AD0 high = 0x69
+
 MPU6050 mpu;
 
 /* =========================================================================
@@ -74,7 +75,6 @@
    http://code.google.com/p/arduino/issues/detail?id=958
  * ========================================================================= */
 
-
 const float M_PI = 3.14159265;
 
 // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
@@ -86,7 +86,7 @@
 // (in degrees) calculated from the quaternions coming from the FIFO.
 // Note that Euler angles suffer from gimbal lock (for more info, see
 // http://en.wikipedia.org/wiki/Gimbal_lock)
-#define OUTPUT_READABLE_EULER
+//#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
@@ -112,11 +112,6 @@
 // format used for the InvenSense teapot demo
 //#define OUTPUT_TEAPOT
 
-
-
-//#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
-//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
@@ -138,9 +133,7 @@
 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);
 
 // ================================================================
@@ -167,15 +160,6 @@
 // ================================================================
 
 void setup() {
-    // join I2C bus (I2Cdev library doesn't do this automatically)
-    //Wire.begin();
-
-    // 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); // wait for Leonardo enumeration, others continue immediately
-
     // 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
@@ -183,46 +167,37 @@
     // crystal solution for the UART timer.
 
     // initialize device
-    //Serial.println(F("Initializing I2C devices..."));
-    //pc.baud(921600);
     pc.printf("Initializing I2C devices...\r\n");
     mpu.initialize();
 
     // verify connection
-    //Serial.println(F("Testing device connections..."));
     pc.printf("Testing device connections...\r\n");
     if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
     else pc.printf("MPU6050 connection failed\r\n");
-    //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
-    wait(2);// Added for test. TODO: Remove this if test goes well.
 
     // load and configure the DMP
-    //Serial.println(F("Initializing DMP..."));
     pc.printf("Initializing DMP...\r\n");
     devStatus = mpu.dmpInitialize();
     
     // make sure it worked (returns 0 if so)
     if (devStatus == 0) {
         // turn on the DMP, now that it's ready
-        //Serial.println(F("Enabling DMP..."));
         pc.printf("Enabling DMP...\r\n");
         mpu.setDMPEnabled(true);
 
         // enable Arduino interrupt detection
-        //Serial.println(F("Enabling interrupt detection..."));
         pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
         checkpin.rise(&dmpDataReady);
         
         mpuIntStatus = mpu.getIntStatus();
 
         // set our DMP Ready flag so the main loop() function knows it's okay to use it
-        //Serial.println(F("DMP ready! Waiting for first interrupt..."));
         pc.printf("DMP ready! Waiting for first interrupt...\r\n");
         dmpReady = true;
 
@@ -233,9 +208,7 @@
         // 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(")"));
+        
         pc.printf("DDMP Initialization failed (code ");
         pc.printf("%d", devStatus);
         pc.printf(")\r\n");
@@ -253,7 +226,6 @@
     // if programming failed, don't try to do anything
     if (!dmpReady) return;
 
-    mpuInterrupt = true;
     // wait for MPU interrupt or extra packet(s) available
     while (!mpuInterrupt && fifoCount < packetSize) {
         // other program behavior stuff here
@@ -296,26 +268,17 @@
         #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);
+            printf("quat\t");
+            printf("%f\t", q.w);
+            printf("%f\t", q.x);
+            printf("%f\t", q.y);
+            printf("%f\t\r\n", 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);
             printf("euler\t");
             printf("%f\t", euler[0] * 180/M_PI);
             printf("%f\t", euler[1] * 180/M_PI);
@@ -327,12 +290,6 @@
             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);
             printf("ypr\t");
             printf("%f\t", ypr[0] * 180/M_PI);
             printf("%f\t", ypr[1] * 180/M_PI);
@@ -345,12 +302,10 @@
             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);
+            printf("areal\t");
+            printf("%f\t", aaReal.x);
+            printf("%f\t", aaReal.y);
+            printf("%f\t\r\n", aaReal.z);
         #endif
 
         #ifdef OUTPUT_READABLE_WORLDACCEL
@@ -360,12 +315,10 @@
             mpu.dmpGetAccel(&aa, fifoBuffer);
             mpu.dmpGetGravity(&gravity, &q);
             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);
+            printf("aworld\t");
+            printf("%f\t", aaWorld.x);
+            printf("%f\t", aaWorld.y);
+            printf("%f\t\r\n", aaWorld.z);
         #endif
     
         #ifdef OUTPUT_TEAPOT
@@ -378,13 +331,13 @@
             teapotPacket[7] = fifoBuffer[9];
             teapotPacket[8] = fifoBuffer[12];
             teapotPacket[9] = fifoBuffer[13];
-            //Serial.write(teapotPacket, 14);
+            for (int i = 0; i < 14; ++i) {
+                pc.send(teapotPacket[i]);
+            }
             teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
         #endif
 
         // blink LED to indicate activity
-        //blinkState = !blinkState;
-        //digitalWrite(LED_PIN, blinkState);
         led1 = !led1;
     }
 }
\ No newline at end of file