NagaokaRoboticsClub_mbedTeam / Mbed 2 deprecated MPU6050Test

Use DMP

Dependencies:   mbed

Fork of MPU6050Test by Panda House

main.cpp@3:e0e7c0150ac1, 2016-08-22 (annotated)

Committer:
tknara
Date:
Mon Aug 22 06:15:08 2016 +0000
Revision:
3:e0e7c0150ac1
Parent:
2:17a6810fc73e 
MPU6050test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syundo0730 0:8d2c753a96e7 1 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
syundo0730 0:8d2c753a96e7 2 // 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
syundo0730 0:8d2c753a96e7 3 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
syundo0730 0:8d2c753a96e7 4 //
syundo0730 0:8d2c753a96e7 5 // Changelog:
syundo0730 0:8d2c753a96e7 6 // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
syundo0730 0:8d2c753a96e7 7 // 2012-06-20 - improved FIFO overflow handling and simplified read process
syundo0730 0:8d2c753a96e7 8 // 2012-06-19 - completely rearranged DMP initialization code and simplification
syundo0730 0:8d2c753a96e7 9 // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
syundo0730 0:8d2c753a96e7 10 // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
syundo0730 0:8d2c753a96e7 11 // 2012-06-05 - add gravity-compensated initial reference frame acceleration output
syundo0730 0:8d2c753a96e7 12 // - add 3D math helper file to DMP6 example sketch
syundo0730 0:8d2c753a96e7 13 // - add Euler output and Yaw/Pitch/Roll output formats
syundo0730 0:8d2c753a96e7 14 // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
syundo0730 0:8d2c753a96e7 15 // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
syundo0730 0:8d2c753a96e7 16 // 2012-05-30 - basic DMP initialization working
tknara 3:e0e7c0150ac1 17
syundo0730 0:8d2c753a96e7 18 /* ============================================
syundo0730 0:8d2c753a96e7 19 I2Cdev device library code is placed under the MIT license
syundo0730 0:8d2c753a96e7 20 Copyright (c) 2012 Jeff Rowberg
tknara 3:e0e7c0150ac1 21
syundo0730 0:8d2c753a96e7 22 Permission is hereby granted, free of charge, to any person obtaining a copy
syundo0730 0:8d2c753a96e7 23 of this software and associated documentation files (the "Software"), to deal
syundo0730 0:8d2c753a96e7 24 in the Software without restriction, including without limitation the rights
syundo0730 0:8d2c753a96e7 25 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
syundo0730 0:8d2c753a96e7 26 copies of the Software, and to permit persons to whom the Software is
syundo0730 0:8d2c753a96e7 27 furnished to do so, subject to the following conditions:
tknara 3:e0e7c0150ac1 28
syundo0730 0:8d2c753a96e7 29 The above copyright notice and this permission notice shall be included in
syundo0730 0:8d2c753a96e7 30 all copies or substantial portions of the Software.
tknara 3:e0e7c0150ac1 31
syundo0730 0:8d2c753a96e7 32 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
syundo0730 0:8d2c753a96e7 33 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
syundo0730 0:8d2c753a96e7 34 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
syundo0730 0:8d2c753a96e7 35 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
syundo0730 0:8d2c753a96e7 36 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
syundo0730 0:8d2c753a96e7 37 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
syundo0730 0:8d2c753a96e7 38 THE SOFTWARE.
syundo0730 0:8d2c753a96e7 39 ===============================================
syundo0730 0:8d2c753a96e7 40 */
tknara 3:e0e7c0150ac1 41
syundo0730 0:8d2c753a96e7 42 #include "I2Cdev.h"
syundo0730 0:8d2c753a96e7 43 #include "MPU6050_6Axis_MotionApps20.h"
tknara 3:e0e7c0150ac1 44
syundo0730 0:8d2c753a96e7 45 MPU6050 mpu;
tknara 3:e0e7c0150ac1 46
syundo0730 0:8d2c753a96e7 47 const float M_PI = 3.14159265;
tknara 3:e0e7c0150ac1 48
PandaHouse 2:17a6810fc73e 49 /* 四元数出力 (DMP生データ) */
PandaHouse 2:17a6810fc73e 50 // #define OUTPUT_READABLE_QUATERNION
PandaHouse 2:17a6810fc73e 51 // Memo:四元数とは、任意の方向を軸とした、任意角の回転を表す概念。
tknara 3:e0e7c0150ac1 52
PandaHouse 2:17a6810fc73e 53 /* オイラー角出力 (四元数->オイラー角算出、ジンバルロック発生に注意) */
PandaHouse 2:17a6810fc73e 54 // #define OUTPUT_READABLE_EULER
PandaHouse 2:17a6810fc73e 55 // Memo:初期姿勢を基準にした座標
tknara 3:e0e7c0150ac1 56
PandaHouse 2:17a6810fc73e 57 /* ロール/ピッチ/ヨー出力 (四元数->重力加速度->RPY算出、ジンバルロック発生に注意) */
PandaHouse 2:17a6810fc73e 58 #define OUTPUT_READABLE_YAWPITCHROLL
PandaHouse 2:17a6810fc73e 59 // Memo:センサーを基準にした座標
tknara 3:e0e7c0150ac1 60
PandaHouse 2:17a6810fc73e 61 /* 重力加速度を除いた加速度(センサ基準座標) */
PandaHouse 2:17a6810fc73e 62 // #define OUTPUT_READABLE_REALACCEL
tknara 3:e0e7c0150ac1 63
PandaHouse 2:17a6810fc73e 64 /* 重力加速度を除いた加速度(初期姿勢と重力加速度を基準にした座標) */
PandaHouse 2:17a6810fc73e 65 // #define OUTPUT_READABLE_WORLDACCEL
tknara 3:e0e7c0150ac1 66
tknara 3:e0e7c0150ac1 67
syundo0730 0:8d2c753a96e7 68 // MPU control/status vars
PandaHouse 2:17a6810fc73e 69 bool dmpReady = false; // DMPの初期化が成功した場合はtrueに設定
PandaHouse 2:17a6810fc73e 70 uint8_t mpuIntStatus; // センサの割り込みステータスを保持
PandaHouse 2:17a6810fc73e 71 uint8_t devStatus; // デバイス動作後の状態 (0 = success, !0 = error)
syundo0730 0:8d2c753a96e7 72 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
syundo0730 0:8d2c753a96e7 73 uint16_t fifoCount; // count of all bytes currently in FIFO
syundo0730 0:8d2c753a96e7 74 uint8_t fifoBuffer[64]; // FIFO storage buffer
tknara 3:e0e7c0150ac1 75
syundo0730 0:8d2c753a96e7 76 // orientation/motion vars
syundo0730 0:8d2c753a96e7 77 Quaternion q; // [w, x, y, z] quaternion container
syundo0730 0:8d2c753a96e7 78 VectorInt16 aa; // [x, y, z] accel sensor measurements
syundo0730 0:8d2c753a96e7 79 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
syundo0730 0:8d2c753a96e7 80 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
syundo0730 0:8d2c753a96e7 81 VectorFloat gravity; // [x, y, z] gravity vector
syundo0730 0:8d2c753a96e7 82 float euler[3]; // [psi, theta, phi] Euler angle container
syundo0730 0:8d2c753a96e7 83 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
tknara 3:e0e7c0150ac1 84
tknara 3:e0e7c0150ac1 85
syundo0730 0:8d2c753a96e7 86 DigitalOut led1(LED1);
tknara 3:e0e7c0150ac1 87 InterruptIn checkpin(p25);
syundo0730 0:8d2c753a96e7 88 Serial pc(USBTX, USBRX);
tknara 3:e0e7c0150ac1 89
syundo0730 0:8d2c753a96e7 90 // ================================================================
PandaHouse 2:17a6810fc73e 91 // === 割り込み検出ルーチン ===
syundo0730 0:8d2c753a96e7 92 // ================================================================
tknara 3:e0e7c0150ac1 93
PandaHouse 2:17a6810fc73e 94 volatile bool mpuInterrupt = false; // センサの割り込みピンがHighになったかどうかを示します
syundo0730 0:8d2c753a96e7 95 void dmpDataReady() {
syundo0730 0:8d2c753a96e7 96 mpuInterrupt = true;
syundo0730 0:8d2c753a96e7 97 }
tknara 3:e0e7c0150ac1 98
tknara 3:e0e7c0150ac1 99
PandaHouse 2:17a6810fc73e 100 // ================================================================
PandaHouse 2:17a6810fc73e 101 // === メインルーチン ===
PandaHouse 2:17a6810fc73e 102 // ================================================================
syundo0730 0:8d2c753a96e7 103 void setup();
syundo0730 0:8d2c753a96e7 104 void loop();
tknara 3:e0e7c0150ac1 105
syundo0730 0:8d2c753a96e7 106 int main() {
syundo0730 0:8d2c753a96e7 107 setup();
syundo0730 0:8d2c753a96e7 108 while(1) {
syundo0730 0:8d2c753a96e7 109 loop();
syundo0730 0:8d2c753a96e7 110 }
syundo0730 0:8d2c753a96e7 111 }
tknara 3:e0e7c0150ac1 112
syundo0730 0:8d2c753a96e7 113 // ================================================================
syundo0730 0:8d2c753a96e7 114 // === INITIAL SETUP ===
syundo0730 0:8d2c753a96e7 115 // ================================================================
tknara 3:e0e7c0150ac1 116
syundo0730 0:8d2c753a96e7 117 void setup() {
tknara 3:e0e7c0150ac1 118
syundo0730 0:8d2c753a96e7 119 // initialize device
syundo0730 0:8d2c753a96e7 120 pc.printf("Initializing I2C devices...\r\n");
syundo0730 0:8d2c753a96e7 121 mpu.initialize();
tknara 3:e0e7c0150ac1 122
syundo0730 0:8d2c753a96e7 123 // verify connection
syundo0730 0:8d2c753a96e7 124 pc.printf("Testing device connections...\r\n");
syundo0730 0:8d2c753a96e7 125 if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
syundo0730 0:8d2c753a96e7 126 else pc.printf("MPU6050 connection failed\r\n");
syundo0730 0:8d2c753a96e7 127
syundo0730 0:8d2c753a96e7 128 // wait for ready
syundo0730 0:8d2c753a96e7 129 //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
syundo0730 0:8d2c753a96e7 130 //while (Serial.available() && Serial.read()); // empty buffer
syundo0730 0:8d2c753a96e7 131 //while (!Serial.available()); // wait for data
syundo0730 0:8d2c753a96e7 132 //while (Serial.available() && Serial.read()); // empty buffer again
tknara 3:e0e7c0150ac1 133
syundo0730 0:8d2c753a96e7 134 // load and configure the DMP
syundo0730 0:8d2c753a96e7 135 pc.printf("Initializing DMP...\r\n");
syundo0730 0:8d2c753a96e7 136 devStatus = mpu.dmpInitialize();
syundo0730 0:8d2c753a96e7 137
syundo0730 0:8d2c753a96e7 138 // make sure it worked (returns 0 if so)
syundo0730 0:8d2c753a96e7 139 if (devStatus == 0) {
syundo0730 0:8d2c753a96e7 140 // turn on the DMP, now that it's ready
syundo0730 0:8d2c753a96e7 141 pc.printf("Enabling DMP...\r\n");
syundo0730 0:8d2c753a96e7 142 mpu.setDMPEnabled(true);
tknara 3:e0e7c0150ac1 143
syundo0730 0:8d2c753a96e7 144 // enable Arduino interrupt detection
syundo0730 0:8d2c753a96e7 145 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
syundo0730 0:8d2c753a96e7 146 checkpin.rise(&dmpDataReady);
syundo0730 0:8d2c753a96e7 147
syundo0730 0:8d2c753a96e7 148 mpuIntStatus = mpu.getIntStatus();
tknara 3:e0e7c0150ac1 149
syundo0730 0:8d2c753a96e7 150 // set our DMP Ready flag so the main loop() function knows it's okay to use it
syundo0730 0:8d2c753a96e7 151 pc.printf("DMP ready! Waiting for first interrupt...\r\n");
syundo0730 0:8d2c753a96e7 152 dmpReady = true;
tknara 3:e0e7c0150ac1 153
syundo0730 0:8d2c753a96e7 154 // get expected DMP packet size for later comparison
syundo0730 0:8d2c753a96e7 155 packetSize = mpu.dmpGetFIFOPacketSize();
syundo0730 0:8d2c753a96e7 156 } else {
syundo0730 0:8d2c753a96e7 157 // ERROR!
syundo0730 0:8d2c753a96e7 158 // 1 = initial memory load failed
syundo0730 0:8d2c753a96e7 159 // 2 = DMP configuration updates failed
syundo0730 0:8d2c753a96e7 160 // (if it's going to break, usually the code will be 1)
syundo0730 1:ec0a08108442 161
syundo0730 0:8d2c753a96e7 162 pc.printf("DDMP Initialization failed (code ");
syundo0730 0:8d2c753a96e7 163 pc.printf("%d", devStatus);
syundo0730 0:8d2c753a96e7 164 pc.printf(")\r\n");
syundo0730 0:8d2c753a96e7 165 }
tknara 3:e0e7c0150ac1 166
syundo0730 0:8d2c753a96e7 167 }
tknara 3:e0e7c0150ac1 168
tknara 3:e0e7c0150ac1 169
tknara 3:e0e7c0150ac1 170
syundo0730 0:8d2c753a96e7 171 // ================================================================
syundo0730 0:8d2c753a96e7 172 // === MAIN PROGRAM LOOP ===
syundo0730 0:8d2c753a96e7 173 // ================================================================
tknara 3:e0e7c0150ac1 174
syundo0730 0:8d2c753a96e7 175 void loop() {
PandaHouse 2:17a6810fc73e 176 // DMPの初期化に失敗した場合、何もしない
syundo0730 0:8d2c753a96e7 177 if (!dmpReady) return;
tknara 3:e0e7c0150ac1 178
PandaHouse 2:17a6810fc73e 179 // センサー割り込み or fifoオーバーフロー待ち
syundo0730 0:8d2c753a96e7 180 while (!mpuInterrupt && fifoCount < packetSize) {
syundo0730 0:8d2c753a96e7 181 // other program behavior stuff here
syundo0730 0:8d2c753a96e7 182 // .
syundo0730 0:8d2c753a96e7 183 // .
syundo0730 0:8d2c753a96e7 184 // .
syundo0730 0:8d2c753a96e7 185 // if you are really paranoid you can frequently test in between other
syundo0730 0:8d2c753a96e7 186 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
syundo0730 0:8d2c753a96e7 187 // while() loop to immediately process the MPU data
syundo0730 0:8d2c753a96e7 188 // .
syundo0730 0:8d2c753a96e7 189 // .
syundo0730 0:8d2c753a96e7 190 // .
syundo0730 0:8d2c753a96e7 191 }
tknara 3:e0e7c0150ac1 192
syundo0730 0:8d2c753a96e7 193 // reset interrupt flag and get INT_STATUS byte
syundo0730 0:8d2c753a96e7 194 mpuInterrupt = false;
syundo0730 0:8d2c753a96e7 195 mpuIntStatus = mpu.getIntStatus();
tknara 3:e0e7c0150ac1 196
syundo0730 0:8d2c753a96e7 197 // get current FIFO count
syundo0730 0:8d2c753a96e7 198 fifoCount = mpu.getFIFOCount();
tknara 3:e0e7c0150ac1 199
PandaHouse 2:17a6810fc73e 200 // オーバーフローをチェック (我々のコードがあまりにも非効率的でない限り、これが起こることはありません)
syundo0730 0:8d2c753a96e7 201 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
syundo0730 0:8d2c753a96e7 202 // reset so we can continue cleanly
syundo0730 0:8d2c753a96e7 203 mpu.resetFIFO();
syundo0730 0:8d2c753a96e7 204 //Serial.println(F("FIFO overflow!"));
tknara 3:e0e7c0150ac1 205
PandaHouse 2:17a6810fc73e 206 // オーバーフローが無ければ、DMPデータ・レディ割り込みをチェック
syundo0730 0:8d2c753a96e7 207 } else if (mpuIntStatus & 0x02) {
syundo0730 0:8d2c753a96e7 208 // wait for correct available data length, should be a VERY short wait
syundo0730 0:8d2c753a96e7 209 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
tknara 3:e0e7c0150ac1 210
syundo0730 0:8d2c753a96e7 211 // read a packet from FIFO
syundo0730 0:8d2c753a96e7 212 mpu.getFIFOBytes(fifoBuffer, packetSize);
syundo0730 0:8d2c753a96e7 213
syundo0730 0:8d2c753a96e7 214 // track FIFO count here in case there is > 1 packet available
syundo0730 0:8d2c753a96e7 215 // (this lets us immediately read more without waiting for an interrupt)
syundo0730 0:8d2c753a96e7 216 fifoCount -= packetSize;
tknara 3:e0e7c0150ac1 217
syundo0730 0:8d2c753a96e7 218 #ifdef OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 219 // display quaternion values in easy matrix form: w x y z
syundo0730 0:8d2c753a96e7 220 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 1:ec0a08108442 221 printf("quat\t");
syundo0730 1:ec0a08108442 222 printf("%f\t", q.w);
syundo0730 1:ec0a08108442 223 printf("%f\t", q.x);
syundo0730 1:ec0a08108442 224 printf("%f\t", q.y);
syundo0730 1:ec0a08108442 225 printf("%f\t\r\n", q.z);
syundo0730 0:8d2c753a96e7 226 #endif
tknara 3:e0e7c0150ac1 227
syundo0730 0:8d2c753a96e7 228 #ifdef OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 229 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 230 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 231 mpu.dmpGetEuler(euler, &q);
syundo0730 0:8d2c753a96e7 232 printf("euler\t");
syundo0730 0:8d2c753a96e7 233 printf("%f\t", euler[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 234 printf("%f\t", euler[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 235 printf("%f\t\r\n", euler[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 236 #endif
tknara 3:e0e7c0150ac1 237
syundo0730 0:8d2c753a96e7 238 #ifdef OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 239 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 240 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 241 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 242 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
syundo0730 0:8d2c753a96e7 243 printf("ypr\t");
syundo0730 0:8d2c753a96e7 244 printf("%f\t", ypr[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 245 printf("%f\t", ypr[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 246 printf("%f\t\r\n", ypr[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 247 #endif
tknara 3:e0e7c0150ac1 248
syundo0730 0:8d2c753a96e7 249 #ifdef OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 250 // display real acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 251 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 252 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 253 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 254 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
syundo0730 1:ec0a08108442 255 printf("areal\t");
PandaHouse 2:17a6810fc73e 256 printf("%d\t", aaReal.x);
PandaHouse 2:17a6810fc73e 257 printf("%d\t", aaReal.y);
PandaHouse 2:17a6810fc73e 258 printf("%d\t\r\n", aaReal.z);
syundo0730 0:8d2c753a96e7 259 #endif
tknara 3:e0e7c0150ac1 260
syundo0730 0:8d2c753a96e7 261 #ifdef OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 262 // display initial world-frame acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 263 // and rotated based on known orientation from quaternion
syundo0730 0:8d2c753a96e7 264 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 265 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 266 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 267 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
syundo0730 1:ec0a08108442 268 printf("aworld\t");
PandaHouse 2:17a6810fc73e 269 printf("%d\t", aaWorld.x);
PandaHouse 2:17a6810fc73e 270 printf("%d\t", aaWorld.y);
PandaHouse 2:17a6810fc73e 271 printf("%d\t\r\n", aaWorld.z);
syundo0730 0:8d2c753a96e7 272 #endif
tknara 3:e0e7c0150ac1 273
syundo0730 0:8d2c753a96e7 274 // blink LED to indicate activity
syundo0730 0:8d2c753a96e7 275 led1 = !led1;
syundo0730 0:8d2c753a96e7 276 }
syundo0730 0:8d2c753a96e7 277 }