Carter Sharer / Mbed 2 deprecated ESE519_Lab6_part3_skeleton

ESE519 Lab6 Part3

Dependencies:   MPU6050_Lab6_Part3 mbed

Fork of BroBot_v2 by Carter Sharer

main.cpp@1:ec0a08108442, 2013-11-23 (annotated)

Committer:
syundo0730
Date:
Sat Nov 23 16:47:59 2013 +0000
Revision:
1:ec0a08108442
Parent:
0:8d2c753a96e7
Child:
2:42c4f3a7813f
comments were added and modified

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
syundo0730 0:8d2c753a96e7 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
syundo0730 0:8d2c753a96e7 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:
syundo0730 0:8d2c753a96e7 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.
syundo0730 0:8d2c753a96e7 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 */
syundo0730 0:8d2c753a96e7 41
syundo0730 0:8d2c753a96e7 42 // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
syundo0730 0:8d2c753a96e7 43 // is used in I2Cdev.h
syundo0730 0:8d2c753a96e7 44 //#include "Wire.h"
syundo0730 0:8d2c753a96e7 45
syundo0730 0:8d2c753a96e7 46 // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
syundo0730 0:8d2c753a96e7 47 // for both classes must be in the include path of your project
syundo0730 0:8d2c753a96e7 48 #include "I2Cdev.h"
syundo0730 0:8d2c753a96e7 49
syundo0730 0:8d2c753a96e7 50 #include "MPU6050_6Axis_MotionApps20.h"
syundo0730 0:8d2c753a96e7 51 //#include "MPU6050.h" // not necessary if using MotionApps include file
syundo0730 0:8d2c753a96e7 52
syundo0730 0:8d2c753a96e7 53 // class default I2C address is 0x68
syundo0730 0:8d2c753a96e7 54 // specific I2C addresses may be passed as a parameter here
syundo0730 0:8d2c753a96e7 55 // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
syundo0730 0:8d2c753a96e7 56 // AD0 high = 0x69
syundo0730 1:ec0a08108442 57
syundo0730 0:8d2c753a96e7 58 MPU6050 mpu;
syundo0730 0:8d2c753a96e7 59
syundo0730 0:8d2c753a96e7 60 /* =========================================================================
syundo0730 0:8d2c753a96e7 61 NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
syundo0730 0:8d2c753a96e7 62 depends on the MPU-6050's INT pin being connected to the Arduino's
syundo0730 0:8d2c753a96e7 63 external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
syundo0730 0:8d2c753a96e7 64 digital I/O pin 2.
syundo0730 0:8d2c753a96e7 65 * ========================================================================= */
syundo0730 0:8d2c753a96e7 66
syundo0730 0:8d2c753a96e7 67 /* =========================================================================
syundo0730 0:8d2c753a96e7 68 NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
syundo0730 0:8d2c753a96e7 69 when using Serial.write(buf, len). The Teapot output uses this method.
syundo0730 0:8d2c753a96e7 70 The solution requires a modification to the Arduino USBAPI.h file, which
syundo0730 0:8d2c753a96e7 71 is fortunately simple, but annoying. This will be fixed in the next IDE
syundo0730 0:8d2c753a96e7 72 release. For more info, see these links:
syundo0730 0:8d2c753a96e7 73
syundo0730 0:8d2c753a96e7 74 http://arduino.cc/forum/index.php/topic,109987.0.html
syundo0730 0:8d2c753a96e7 75 http://code.google.com/p/arduino/issues/detail?id=958
syundo0730 0:8d2c753a96e7 76 * ========================================================================= */
syundo0730 0:8d2c753a96e7 77
syundo0730 0:8d2c753a96e7 78 const float M_PI = 3.14159265;
syundo0730 0:8d2c753a96e7 79
syundo0730 0:8d2c753a96e7 80 // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
syundo0730 0:8d2c753a96e7 81 // quaternion components in a [w, x, y, z] format (not best for parsing
syundo0730 0:8d2c753a96e7 82 // on a remote host such as Processing or something though)
syundo0730 0:8d2c753a96e7 83 //#define OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 84
syundo0730 0:8d2c753a96e7 85 // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
syundo0730 0:8d2c753a96e7 86 // (in degrees) calculated from the quaternions coming from the FIFO.
syundo0730 0:8d2c753a96e7 87 // Note that Euler angles suffer from gimbal lock (for more info, see
syundo0730 0:8d2c753a96e7 88 // http://en.wikipedia.org/wiki/Gimbal_lock)
syundo0730 1:ec0a08108442 89 //#define OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 90
syundo0730 0:8d2c753a96e7 91 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
syundo0730 0:8d2c753a96e7 92 // pitch/roll angles (in degrees) calculated from the quaternions coming
syundo0730 0:8d2c753a96e7 93 // from the FIFO. Note this also requires gravity vector calculations.
syundo0730 0:8d2c753a96e7 94 // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
syundo0730 0:8d2c753a96e7 95 // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
syundo0730 0:8d2c753a96e7 96 //#define OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 97
syundo0730 0:8d2c753a96e7 98 // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
syundo0730 0:8d2c753a96e7 99 // components with gravity removed. This acceleration reference frame is
syundo0730 0:8d2c753a96e7 100 // not compensated for orientation, so +X is always +X according to the
syundo0730 0:8d2c753a96e7 101 // sensor, just without the effects of gravity. If you want acceleration
syundo0730 0:8d2c753a96e7 102 // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
syundo0730 0:8d2c753a96e7 103 //#define OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 104
syundo0730 0:8d2c753a96e7 105 // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
syundo0730 0:8d2c753a96e7 106 // components with gravity removed and adjusted for the world frame of
syundo0730 0:8d2c753a96e7 107 // reference (yaw is relative to initial orientation, since no magnetometer
syundo0730 0:8d2c753a96e7 108 // is present in this case). Could be quite handy in some cases.
syundo0730 0:8d2c753a96e7 109 //#define OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 110
syundo0730 0:8d2c753a96e7 111 // uncomment "OUTPUT_TEAPOT" if you want output that matches the
syundo0730 0:8d2c753a96e7 112 // format used for the InvenSense teapot demo
syundo0730 0:8d2c753a96e7 113 //#define OUTPUT_TEAPOT
syundo0730 0:8d2c753a96e7 114
syundo0730 0:8d2c753a96e7 115 // MPU control/status vars
syundo0730 0:8d2c753a96e7 116 bool dmpReady = false; // set true if DMP init was successful
syundo0730 0:8d2c753a96e7 117 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
syundo0730 0:8d2c753a96e7 118 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
syundo0730 0:8d2c753a96e7 119 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
syundo0730 0:8d2c753a96e7 120 uint16_t fifoCount; // count of all bytes currently in FIFO
syundo0730 0:8d2c753a96e7 121 uint8_t fifoBuffer[64]; // FIFO storage buffer
syundo0730 0:8d2c753a96e7 122
syundo0730 0:8d2c753a96e7 123 // orientation/motion vars
syundo0730 0:8d2c753a96e7 124 Quaternion q; // [w, x, y, z] quaternion container
syundo0730 0:8d2c753a96e7 125 VectorInt16 aa; // [x, y, z] accel sensor measurements
syundo0730 0:8d2c753a96e7 126 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
syundo0730 0:8d2c753a96e7 127 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
syundo0730 0:8d2c753a96e7 128 VectorFloat gravity; // [x, y, z] gravity vector
syundo0730 0:8d2c753a96e7 129 float euler[3]; // [psi, theta, phi] Euler angle container
syundo0730 0:8d2c753a96e7 130 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
syundo0730 0:8d2c753a96e7 131
syundo0730 0:8d2c753a96e7 132 // packet structure for InvenSense teapot demo
syundo0730 0:8d2c753a96e7 133 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
syundo0730 0:8d2c753a96e7 134
syundo0730 0:8d2c753a96e7 135 DigitalOut led1(LED1);
syundo0730 0:8d2c753a96e7 136 InterruptIn checkpin(p29);
syundo0730 0:8d2c753a96e7 137 Serial pc(USBTX, USBRX);
syundo0730 0:8d2c753a96e7 138
syundo0730 0:8d2c753a96e7 139 // ================================================================
syundo0730 0:8d2c753a96e7 140 // === INTERRUPT DETECTION ROUTINE ===
syundo0730 0:8d2c753a96e7 141 // ================================================================
syundo0730 0:8d2c753a96e7 142
syundo0730 0:8d2c753a96e7 143 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
syundo0730 0:8d2c753a96e7 144 void dmpDataReady() {
syundo0730 0:8d2c753a96e7 145 mpuInterrupt = true;
syundo0730 0:8d2c753a96e7 146 }
syundo0730 0:8d2c753a96e7 147
syundo0730 0:8d2c753a96e7 148 void setup();
syundo0730 0:8d2c753a96e7 149 void loop();
syundo0730 0:8d2c753a96e7 150
syundo0730 0:8d2c753a96e7 151 int main() {
syundo0730 0:8d2c753a96e7 152 setup();
syundo0730 0:8d2c753a96e7 153 while(1) {
syundo0730 0:8d2c753a96e7 154 loop();
syundo0730 0:8d2c753a96e7 155 }
syundo0730 0:8d2c753a96e7 156 }
syundo0730 0:8d2c753a96e7 157
syundo0730 0:8d2c753a96e7 158 // ================================================================
syundo0730 0:8d2c753a96e7 159 // === INITIAL SETUP ===
syundo0730 0:8d2c753a96e7 160 // ================================================================
syundo0730 0:8d2c753a96e7 161
syundo0730 0:8d2c753a96e7 162 void setup() {
syundo0730 0:8d2c753a96e7 163 // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
syundo0730 0:8d2c753a96e7 164 // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
syundo0730 0:8d2c753a96e7 165 // the baud timing being too misaligned with processor ticks. You must use
syundo0730 0:8d2c753a96e7 166 // 38400 or slower in these cases, or use some kind of external separate
syundo0730 0:8d2c753a96e7 167 // crystal solution for the UART timer.
syundo0730 0:8d2c753a96e7 168
syundo0730 0:8d2c753a96e7 169 // initialize device
syundo0730 0:8d2c753a96e7 170 pc.printf("Initializing I2C devices...\r\n");
syundo0730 0:8d2c753a96e7 171 mpu.initialize();
syundo0730 0:8d2c753a96e7 172
syundo0730 0:8d2c753a96e7 173 // verify connection
syundo0730 0:8d2c753a96e7 174 pc.printf("Testing device connections...\r\n");
syundo0730 0:8d2c753a96e7 175 if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
syundo0730 0:8d2c753a96e7 176 else pc.printf("MPU6050 connection failed\r\n");
syundo0730 0:8d2c753a96e7 177
syundo0730 0:8d2c753a96e7 178 // wait for ready
syundo0730 0:8d2c753a96e7 179 //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
syundo0730 0:8d2c753a96e7 180 //while (Serial.available() && Serial.read()); // empty buffer
syundo0730 0:8d2c753a96e7 181 //while (!Serial.available()); // wait for data
syundo0730 0:8d2c753a96e7 182 //while (Serial.available() && Serial.read()); // empty buffer again
syundo0730 0:8d2c753a96e7 183
syundo0730 0:8d2c753a96e7 184 // load and configure the DMP
syundo0730 0:8d2c753a96e7 185 pc.printf("Initializing DMP...\r\n");
syundo0730 0:8d2c753a96e7 186 devStatus = mpu.dmpInitialize();
syundo0730 0:8d2c753a96e7 187
syundo0730 0:8d2c753a96e7 188 // make sure it worked (returns 0 if so)
syundo0730 0:8d2c753a96e7 189 if (devStatus == 0) {
syundo0730 0:8d2c753a96e7 190 // turn on the DMP, now that it's ready
syundo0730 0:8d2c753a96e7 191 pc.printf("Enabling DMP...\r\n");
syundo0730 0:8d2c753a96e7 192 mpu.setDMPEnabled(true);
syundo0730 0:8d2c753a96e7 193
syundo0730 0:8d2c753a96e7 194 // enable Arduino interrupt detection
syundo0730 0:8d2c753a96e7 195 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
syundo0730 0:8d2c753a96e7 196 checkpin.rise(&dmpDataReady);
syundo0730 0:8d2c753a96e7 197
syundo0730 0:8d2c753a96e7 198 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 199
syundo0730 0:8d2c753a96e7 200 // set our DMP Ready flag so the main loop() function knows it's okay to use it
syundo0730 0:8d2c753a96e7 201 pc.printf("DMP ready! Waiting for first interrupt...\r\n");
syundo0730 0:8d2c753a96e7 202 dmpReady = true;
syundo0730 0:8d2c753a96e7 203
syundo0730 0:8d2c753a96e7 204 // get expected DMP packet size for later comparison
syundo0730 0:8d2c753a96e7 205 packetSize = mpu.dmpGetFIFOPacketSize();
syundo0730 0:8d2c753a96e7 206 } else {
syundo0730 0:8d2c753a96e7 207 // ERROR!
syundo0730 0:8d2c753a96e7 208 // 1 = initial memory load failed
syundo0730 0:8d2c753a96e7 209 // 2 = DMP configuration updates failed
syundo0730 0:8d2c753a96e7 210 // (if it's going to break, usually the code will be 1)
syundo0730 1:ec0a08108442 211
syundo0730 0:8d2c753a96e7 212 pc.printf("DDMP Initialization failed (code ");
syundo0730 0:8d2c753a96e7 213 pc.printf("%d", devStatus);
syundo0730 0:8d2c753a96e7 214 pc.printf(")\r\n");
syundo0730 0:8d2c753a96e7 215 }
syundo0730 0:8d2c753a96e7 216
syundo0730 0:8d2c753a96e7 217 }
syundo0730 0:8d2c753a96e7 218
syundo0730 0:8d2c753a96e7 219
syundo0730 0:8d2c753a96e7 220
syundo0730 0:8d2c753a96e7 221 // ================================================================
syundo0730 0:8d2c753a96e7 222 // === MAIN PROGRAM LOOP ===
syundo0730 0:8d2c753a96e7 223 // ================================================================
syundo0730 0:8d2c753a96e7 224
syundo0730 0:8d2c753a96e7 225 void loop() {
syundo0730 0:8d2c753a96e7 226 // if programming failed, don't try to do anything
syundo0730 0:8d2c753a96e7 227 if (!dmpReady) return;
syundo0730 0:8d2c753a96e7 228
syundo0730 0:8d2c753a96e7 229 // wait for MPU interrupt or extra packet(s) available
syundo0730 0:8d2c753a96e7 230 while (!mpuInterrupt && fifoCount < packetSize) {
syundo0730 0:8d2c753a96e7 231 // other program behavior stuff here
syundo0730 0:8d2c753a96e7 232 // .
syundo0730 0:8d2c753a96e7 233 // .
syundo0730 0:8d2c753a96e7 234 // .
syundo0730 0:8d2c753a96e7 235 // if you are really paranoid you can frequently test in between other
syundo0730 0:8d2c753a96e7 236 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
syundo0730 0:8d2c753a96e7 237 // while() loop to immediately process the MPU data
syundo0730 0:8d2c753a96e7 238 // .
syundo0730 0:8d2c753a96e7 239 // .
syundo0730 0:8d2c753a96e7 240 // .
syundo0730 0:8d2c753a96e7 241 }
syundo0730 0:8d2c753a96e7 242
syundo0730 0:8d2c753a96e7 243 // reset interrupt flag and get INT_STATUS byte
syundo0730 0:8d2c753a96e7 244 mpuInterrupt = false;
syundo0730 0:8d2c753a96e7 245 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 246
syundo0730 0:8d2c753a96e7 247 // get current FIFO count
syundo0730 0:8d2c753a96e7 248 fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 249
syundo0730 0:8d2c753a96e7 250 // check for overflow (this should never happen unless our code is too inefficient)
syundo0730 0:8d2c753a96e7 251 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
syundo0730 0:8d2c753a96e7 252 // reset so we can continue cleanly
syundo0730 0:8d2c753a96e7 253 mpu.resetFIFO();
syundo0730 0:8d2c753a96e7 254 //Serial.println(F("FIFO overflow!"));
syundo0730 0:8d2c753a96e7 255
syundo0730 0:8d2c753a96e7 256 // otherwise, check for DMP data ready interrupt (this should happen frequently)
syundo0730 0:8d2c753a96e7 257 } else if (mpuIntStatus & 0x02) {
syundo0730 0:8d2c753a96e7 258 // wait for correct available data length, should be a VERY short wait
syundo0730 0:8d2c753a96e7 259 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 260
syundo0730 0:8d2c753a96e7 261 // read a packet from FIFO
syundo0730 0:8d2c753a96e7 262 mpu.getFIFOBytes(fifoBuffer, packetSize);
syundo0730 0:8d2c753a96e7 263
syundo0730 0:8d2c753a96e7 264 // track FIFO count here in case there is > 1 packet available
syundo0730 0:8d2c753a96e7 265 // (this lets us immediately read more without waiting for an interrupt)
syundo0730 0:8d2c753a96e7 266 fifoCount -= packetSize;
syundo0730 0:8d2c753a96e7 267
syundo0730 0:8d2c753a96e7 268 #ifdef OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 269 // display quaternion values in easy matrix form: w x y z
syundo0730 0:8d2c753a96e7 270 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 1:ec0a08108442 271 printf("quat\t");
syundo0730 1:ec0a08108442 272 printf("%f\t", q.w);
syundo0730 1:ec0a08108442 273 printf("%f\t", q.x);
syundo0730 1:ec0a08108442 274 printf("%f\t", q.y);
syundo0730 1:ec0a08108442 275 printf("%f\t\r\n", q.z);
syundo0730 0:8d2c753a96e7 276 #endif
syundo0730 0:8d2c753a96e7 277
syundo0730 0:8d2c753a96e7 278 #ifdef OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 279 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 280 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 281 mpu.dmpGetEuler(euler, &q);
syundo0730 0:8d2c753a96e7 282 printf("euler\t");
syundo0730 0:8d2c753a96e7 283 printf("%f\t", euler[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 284 printf("%f\t", euler[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 285 printf("%f\t\r\n", euler[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 286 #endif
syundo0730 0:8d2c753a96e7 287
syundo0730 0:8d2c753a96e7 288 #ifdef OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 289 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 290 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 291 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 292 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
syundo0730 0:8d2c753a96e7 293 printf("ypr\t");
syundo0730 0:8d2c753a96e7 294 printf("%f\t", ypr[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 295 printf("%f\t", ypr[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 296 printf("%f\t\r\n", ypr[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 297 #endif
syundo0730 0:8d2c753a96e7 298
syundo0730 0:8d2c753a96e7 299 #ifdef OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 300 // display real acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 301 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 302 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 303 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 304 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
syundo0730 1:ec0a08108442 305 printf("areal\t");
syundo0730 1:ec0a08108442 306 printf("%f\t", aaReal.x);
syundo0730 1:ec0a08108442 307 printf("%f\t", aaReal.y);
syundo0730 1:ec0a08108442 308 printf("%f\t\r\n", aaReal.z);
syundo0730 0:8d2c753a96e7 309 #endif
syundo0730 0:8d2c753a96e7 310
syundo0730 0:8d2c753a96e7 311 #ifdef OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 312 // display initial world-frame acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 313 // and rotated based on known orientation from quaternion
syundo0730 0:8d2c753a96e7 314 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 315 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 316 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 317 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
syundo0730 1:ec0a08108442 318 printf("aworld\t");
syundo0730 1:ec0a08108442 319 printf("%f\t", aaWorld.x);
syundo0730 1:ec0a08108442 320 printf("%f\t", aaWorld.y);
syundo0730 1:ec0a08108442 321 printf("%f\t\r\n", aaWorld.z);
syundo0730 0:8d2c753a96e7 322 #endif
syundo0730 0:8d2c753a96e7 323
syundo0730 0:8d2c753a96e7 324 #ifdef OUTPUT_TEAPOT
syundo0730 0:8d2c753a96e7 325 // display quaternion values in InvenSense Teapot demo format:
syundo0730 0:8d2c753a96e7 326 teapotPacket[2] = fifoBuffer[0];
syundo0730 0:8d2c753a96e7 327 teapotPacket[3] = fifoBuffer[1];
syundo0730 0:8d2c753a96e7 328 teapotPacket[4] = fifoBuffer[4];
syundo0730 0:8d2c753a96e7 329 teapotPacket[5] = fifoBuffer[5];
syundo0730 0:8d2c753a96e7 330 teapotPacket[6] = fifoBuffer[8];
syundo0730 0:8d2c753a96e7 331 teapotPacket[7] = fifoBuffer[9];
syundo0730 0:8d2c753a96e7 332 teapotPacket[8] = fifoBuffer[12];
syundo0730 0:8d2c753a96e7 333 teapotPacket[9] = fifoBuffer[13];
syundo0730 1:ec0a08108442 334 for (int i = 0; i < 14; ++i) {
syundo0730 1:ec0a08108442 335 pc.send(teapotPacket[i]);
syundo0730 1:ec0a08108442 336 }
syundo0730 0:8d2c753a96e7 337 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
syundo0730 0:8d2c753a96e7 338 #endif
syundo0730 0:8d2c753a96e7 339
syundo0730 0:8d2c753a96e7 340 // blink LED to indicate activity
syundo0730 0:8d2c753a96e7 341 led1 = !led1;
syundo0730 0:8d2c753a96e7 342 }
syundo0730 0:8d2c753a96e7 343 }