Diff: main.cpp

Revision:

2:17a6810fc73e

Parent:

1:ec0a08108442

--- a/main.cpp	Sat Nov 23 16:47:59 2013 +0000
+++ b/main.cpp	Wed Oct 28 03:37:39 2015 +0000
@@ -39,83 +39,36 @@
 ===============================================
 */
 
-// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
-// is used in I2Cdev.h
-//#include "Wire.h"
-
-// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
-// for both classes must be in the include path of your project
 #include "I2Cdev.h"
-
 #include "MPU6050_6Axis_MotionApps20.h"
-//#include "MPU6050.h" // not necessary if using MotionApps include file
-
-// class default I2C address is 0x68
-// 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;
 
-/* =========================================================================
-   NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
-   depends on the MPU-6050's INT pin being connected to the Arduino's
-   external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
-   digital I/O pin 2.
- * ========================================================================= */
-
-/* =========================================================================
-   NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
-   when using Serial.write(buf, len). The Teapot output uses this method.
-   The solution requires a modification to the Arduino USBAPI.h file, which
-   is fortunately simple, but annoying. This will be fixed in the next IDE
-   release. For more info, see these links:
-
-   http://arduino.cc/forum/index.php/topic,109987.0.html
-   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
-// 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
+/* 四元数出力 (DMP生データ) */
+// #define OUTPUT_READABLE_QUATERNION
+// Memo:四元数とは、任意の方向を軸とした、任意角の回転を表す概念。
 
-// 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 (for more info, see
-// http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_EULER
+/* オイラー角出力 (四元数->オイラー角算出、ジンバルロック発生に注意) */
+// #define OUTPUT_READABLE_EULER
+// Memo:初期姿勢を基準にした座標
 
-// 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 (for
-// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_YAWPITCHROLL
+/* ロール/ピッチ/ヨー出力 (四元数->重力加速度->RPY算出、ジンバルロック発生に注意) */
+#define OUTPUT_READABLE_YAWPITCHROLL
+// Memo:センサーを基準にした座標
 
-// 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
+/* 重力加速度を除いた加速度(センサ基準座標) */
+// #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
+/* 重力加速度を除いた加速度(初期姿勢と重力加速度を基準にした座標) */
+// #define OUTPUT_READABLE_WORLDACCEL
 
-// uncomment "OUTPUT_TEAPOT" if you want output that matches the
-// format used for the InvenSense teapot demo
-//#define OUTPUT_TEAPOT
 
 // 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)
+bool dmpReady = false;  // DMPの初期化が成功した場合はtrueに設定
+uint8_t mpuIntStatus;   // センサの割り込みステータスを保持
+uint8_t devStatus;      // デバイス動作後の状態 (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
@@ -129,22 +82,24 @@
 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);
 
 // ================================================================
-// ===               INTERRUPT DETECTION ROUTINE                ===
+// ===                     割り込み検出ルーチン                    ===
 // ================================================================
 
-volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+volatile bool mpuInterrupt = false;     // センサの割り込みピンがHighになったかどうかを示します
 void dmpDataReady() {
     mpuInterrupt = true;
 }
 
+
+// ================================================================
+// ===                        メインルーチン                      ===
+// ================================================================
 void setup();
 void loop();
 
@@ -160,11 +115,6 @@
 // ================================================================
 
 void setup() {
-    // 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
     pc.printf("Initializing I2C devices...\r\n");
@@ -223,10 +173,10 @@
 // ================================================================
 
 void loop() {
-    // if programming failed, don't try to do anything
+    // DMPの初期化に失敗した場合、何もしない
     if (!dmpReady) return;
 
-    // wait for MPU interrupt or extra packet(s) available
+    // センサー割り込み or fifoオーバーフロー待ち
     while (!mpuInterrupt && fifoCount < packetSize) {
         // other program behavior stuff here
         // .
@@ -247,13 +197,13 @@
     // get current FIFO count
     fifoCount = mpu.getFIFOCount();
 
-    // check for overflow (this should never happen unless our code is too inefficient)
+    // オーバーフローをチェック (我々のコードがあまりにも非効率的でない限り、これが起こることはありません)
     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
         // reset so we can continue cleanly
         mpu.resetFIFO();
         //Serial.println(F("FIFO overflow!"));
 
-    // otherwise, check for DMP data ready interrupt (this should happen frequently)
+    // オーバーフローが無ければ、DMPデータ・レディ割り込みをチェック
     } else if (mpuIntStatus & 0x02) {
         // wait for correct available data length, should be a VERY short wait
         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
@@ -303,9 +253,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("%d\t", aaReal.x);
+            printf("%d\t", aaReal.y);
+            printf("%d\t\r\n", aaReal.z);
         #endif
 
         #ifdef OUTPUT_READABLE_WORLDACCEL
@@ -316,25 +266,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);
-        #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
+            printf("%d\t", aaWorld.x);
+            printf("%d\t", aaWorld.y);
+            printf("%d\t\r\n", aaWorld.z);
         #endif
 
         // blink LED to indicate activity