Diff: main.cpp

Revision:

4:854f7a1418f6

Parent:

3:e0e7c0150ac1

--- a/main.cpp	Mon Aug 22 06:15:08 2016 +0000
+++ b/main.cpp	Wed Nov 09 08:57:21 2016 +0000
@@ -1,43 +1,4 @@
-// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
-// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
-//
-// Changelog:
-//     2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
-//     2012-06-20 - improved FIFO overflow handling and simplified read process
-//     2012-06-19 - completely rearranged DMP initialization code and simplification
-//     2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
-//     2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
-//     2012-06-05 - add gravity-compensated initial reference frame acceleration output
-//                - add 3D math helper file to DMP6 example sketch
-//                - add Euler output and Yaw/Pitch/Roll output formats
-//     2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
-//     2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
-//     2012-05-30 - basic DMP initialization working
- 
-/* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
- 
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
- 
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
- 
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
+
  
 #include "I2Cdev.h"
 #include "MPU6050_6Axis_MotionApps20.h"
@@ -46,24 +7,6 @@
  
 const float M_PI = 3.14159265;
  
-/* 四元数出力 (DMP生データ) */
-// #define OUTPUT_READABLE_QUATERNION
-// Memo:四元数とは、任意の方向を軸とした、任意角の回転を表す概念。
- 
-/* オイラー角出力 (四元数->オイラー角算出、ジンバルロック発生に注意) */
-// #define OUTPUT_READABLE_EULER
-// Memo:初期姿勢を基準にした座標
- 
-/* ロール/ピッチ/ヨー出力 (四元数->重力加速度->RPY算出、ジンバルロック発生に注意) */
-#define OUTPUT_READABLE_YAWPITCHROLL
-// Memo:センサーを基準にした座標
- 
-/* 重力加速度を除いた加速度(センサ基準座標) */
-// #define OUTPUT_READABLE_REALACCEL
- 
-/* 重力加速度を除いた加速度(初期姿勢と重力加速度を基準にした座標) */
-// #define OUTPUT_READABLE_WORLDACCEL
- 
  
 // MPU control/status vars
 bool dmpReady = false;  // DMPの初期化が成功した場合はtrueに設定
@@ -178,16 +121,6 @@
  
     // センサー割り込み or fifoオーバーフロー待ち
     while (!mpuInterrupt && fifoCount < packetSize) {
-        // 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
-        // .
-        // .
-        // .
     }
  
     // reset interrupt flag and get INT_STATUS byte
@@ -214,64 +147,16 @@
         // track FIFO count here in case there is > 1 packet available
         // (this lets us immediately read more without waiting for an interrupt)
         fifoCount -= packetSize;
- 
-        #ifdef OUTPUT_READABLE_QUATERNION
-            // display quaternion values in easy matrix form: w x y z
-            mpu.dmpGetQuaternion(&q, fifoBuffer);
-            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);
-            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);
-        #endif
- 
-        #ifdef OUTPUT_READABLE_YAWPITCHROLL
+
             // display Euler angles in degrees
             mpu.dmpGetQuaternion(&q, fifoBuffer);
             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);
-        #endif
+//            printf("ypr\t");
+            printf("%f,", ypr[0] * 180/M_PI);
+            printf("%f,", ypr[1] * 180/M_PI);
+            printf("%f,\n\r", ypr[2] * 180/M_PI);
+            wait(0.1);
  
-        #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("%d\t", aaReal.x);
-            printf("%d\t", aaReal.y);
-            printf("%d\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("%d\t", aaWorld.x);
-            printf("%d\t", aaWorld.y);
-            printf("%d\t\r\n", aaWorld.z);
-        #endif
- 
-        // blink LED to indicate activity
-        led1 = !led1;
     }
 }
\ No newline at end of file