Carter Sharer / Mbed 2 deprecated BroBot_ESE350_Skeleton

BroBot Code for ESE350 Lab6 part 3 (Skeleton)

Dependencies:   MPU6050_V3 mbed-rtos mbed

Fork of BroBot_RTOS_ESE350 by Carter Sharer

main.cpp@0:8d2c753a96e7, 2013-11-19 (annotated)

Committer:
syundo0730
Date:
Tue Nov 19 19:10:30 2013 +0000
Revision:
0:8d2c753a96e7
Child:
1:ec0a08108442
first commit

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 0:8d2c753a96e7 57 MPU6050 mpu;
syundo0730 0:8d2c753a96e7 58
syundo0730 0:8d2c753a96e7 59 /* =========================================================================
syundo0730 0:8d2c753a96e7 60 NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
syundo0730 0:8d2c753a96e7 61 depends on the MPU-6050's INT pin being connected to the Arduino's
syundo0730 0:8d2c753a96e7 62 external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
syundo0730 0:8d2c753a96e7 63 digital I/O pin 2.
syundo0730 0:8d2c753a96e7 64 * ========================================================================= */
syundo0730 0:8d2c753a96e7 65
syundo0730 0:8d2c753a96e7 66 /* =========================================================================
syundo0730 0:8d2c753a96e7 67 NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
syundo0730 0:8d2c753a96e7 68 when using Serial.write(buf, len). The Teapot output uses this method.
syundo0730 0:8d2c753a96e7 69 The solution requires a modification to the Arduino USBAPI.h file, which
syundo0730 0:8d2c753a96e7 70 is fortunately simple, but annoying. This will be fixed in the next IDE
syundo0730 0:8d2c753a96e7 71 release. For more info, see these links:
syundo0730 0:8d2c753a96e7 72
syundo0730 0:8d2c753a96e7 73 http://arduino.cc/forum/index.php/topic,109987.0.html
syundo0730 0:8d2c753a96e7 74 http://code.google.com/p/arduino/issues/detail?id=958
syundo0730 0:8d2c753a96e7 75 * ========================================================================= */
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 0:8d2c753a96e7 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
syundo0730 0:8d2c753a96e7 116
syundo0730 0:8d2c753a96e7 117 //#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
syundo0730 0:8d2c753a96e7 118 //bool blinkState = false;
syundo0730 0:8d2c753a96e7 119
syundo0730 0:8d2c753a96e7 120 // MPU control/status vars
syundo0730 0:8d2c753a96e7 121 bool dmpReady = false; // set true if DMP init was successful
syundo0730 0:8d2c753a96e7 122 uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
syundo0730 0:8d2c753a96e7 123 uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
syundo0730 0:8d2c753a96e7 124 uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
syundo0730 0:8d2c753a96e7 125 uint16_t fifoCount; // count of all bytes currently in FIFO
syundo0730 0:8d2c753a96e7 126 uint8_t fifoBuffer[64]; // FIFO storage buffer
syundo0730 0:8d2c753a96e7 127
syundo0730 0:8d2c753a96e7 128 // orientation/motion vars
syundo0730 0:8d2c753a96e7 129 Quaternion q; // [w, x, y, z] quaternion container
syundo0730 0:8d2c753a96e7 130 VectorInt16 aa; // [x, y, z] accel sensor measurements
syundo0730 0:8d2c753a96e7 131 VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
syundo0730 0:8d2c753a96e7 132 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
syundo0730 0:8d2c753a96e7 133 VectorFloat gravity; // [x, y, z] gravity vector
syundo0730 0:8d2c753a96e7 134 float euler[3]; // [psi, theta, phi] Euler angle container
syundo0730 0:8d2c753a96e7 135 float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
syundo0730 0:8d2c753a96e7 136
syundo0730 0:8d2c753a96e7 137 // packet structure for InvenSense teapot demo
syundo0730 0:8d2c753a96e7 138 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
syundo0730 0:8d2c753a96e7 139
syundo0730 0:8d2c753a96e7 140 DigitalOut led1(LED1);
syundo0730 0:8d2c753a96e7 141
syundo0730 0:8d2c753a96e7 142 InterruptIn checkpin(p29);
syundo0730 0:8d2c753a96e7 143
syundo0730 0:8d2c753a96e7 144 Serial pc(USBTX, USBRX);
syundo0730 0:8d2c753a96e7 145
syundo0730 0:8d2c753a96e7 146 // ================================================================
syundo0730 0:8d2c753a96e7 147 // === INTERRUPT DETECTION ROUTINE ===
syundo0730 0:8d2c753a96e7 148 // ================================================================
syundo0730 0:8d2c753a96e7 149
syundo0730 0:8d2c753a96e7 150 volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
syundo0730 0:8d2c753a96e7 151 void dmpDataReady() {
syundo0730 0:8d2c753a96e7 152 mpuInterrupt = true;
syundo0730 0:8d2c753a96e7 153 }
syundo0730 0:8d2c753a96e7 154
syundo0730 0:8d2c753a96e7 155 void setup();
syundo0730 0:8d2c753a96e7 156 void loop();
syundo0730 0:8d2c753a96e7 157
syundo0730 0:8d2c753a96e7 158 int main() {
syundo0730 0:8d2c753a96e7 159 setup();
syundo0730 0:8d2c753a96e7 160 while(1) {
syundo0730 0:8d2c753a96e7 161 loop();
syundo0730 0:8d2c753a96e7 162 }
syundo0730 0:8d2c753a96e7 163 }
syundo0730 0:8d2c753a96e7 164
syundo0730 0:8d2c753a96e7 165 // ================================================================
syundo0730 0:8d2c753a96e7 166 // === INITIAL SETUP ===
syundo0730 0:8d2c753a96e7 167 // ================================================================
syundo0730 0:8d2c753a96e7 168
syundo0730 0:8d2c753a96e7 169 void setup() {
syundo0730 0:8d2c753a96e7 170 // join I2C bus (I2Cdev library doesn't do this automatically)
syundo0730 0:8d2c753a96e7 171 //Wire.begin();
syundo0730 0:8d2c753a96e7 172
syundo0730 0:8d2c753a96e7 173 // initialize serial communication
syundo0730 0:8d2c753a96e7 174 // (115200 chosen because it is required for Teapot Demo output, but it's
syundo0730 0:8d2c753a96e7 175 // really up to you depending on your project)
syundo0730 0:8d2c753a96e7 176 //Serial.begin(115200);
syundo0730 0:8d2c753a96e7 177 //while (!Serial); // wait for Leonardo enumeration, others continue immediately
syundo0730 0:8d2c753a96e7 178
syundo0730 0:8d2c753a96e7 179 // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
syundo0730 0:8d2c753a96e7 180 // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
syundo0730 0:8d2c753a96e7 181 // the baud timing being too misaligned with processor ticks. You must use
syundo0730 0:8d2c753a96e7 182 // 38400 or slower in these cases, or use some kind of external separate
syundo0730 0:8d2c753a96e7 183 // crystal solution for the UART timer.
syundo0730 0:8d2c753a96e7 184
syundo0730 0:8d2c753a96e7 185 // initialize device
syundo0730 0:8d2c753a96e7 186 //Serial.println(F("Initializing I2C devices..."));
syundo0730 0:8d2c753a96e7 187 //pc.baud(921600);
syundo0730 0:8d2c753a96e7 188 pc.printf("Initializing I2C devices...\r\n");
syundo0730 0:8d2c753a96e7 189 mpu.initialize();
syundo0730 0:8d2c753a96e7 190
syundo0730 0:8d2c753a96e7 191 // verify connection
syundo0730 0:8d2c753a96e7 192 //Serial.println(F("Testing device connections..."));
syundo0730 0:8d2c753a96e7 193 pc.printf("Testing device connections...\r\n");
syundo0730 0:8d2c753a96e7 194 if (mpu.testConnection()) pc.printf("MPU6050 connection successful\r\n");
syundo0730 0:8d2c753a96e7 195 else pc.printf("MPU6050 connection failed\r\n");
syundo0730 0:8d2c753a96e7 196 //Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
syundo0730 0:8d2c753a96e7 197
syundo0730 0:8d2c753a96e7 198 // wait for ready
syundo0730 0:8d2c753a96e7 199 //Serial.println(F("\nSend any character to begin DMP programming and demo: "));
syundo0730 0:8d2c753a96e7 200 //while (Serial.available() && Serial.read()); // empty buffer
syundo0730 0:8d2c753a96e7 201 //while (!Serial.available()); // wait for data
syundo0730 0:8d2c753a96e7 202 //while (Serial.available() && Serial.read()); // empty buffer again
syundo0730 0:8d2c753a96e7 203 wait(2);// Added for test. TODO: Remove this if test goes well.
syundo0730 0:8d2c753a96e7 204
syundo0730 0:8d2c753a96e7 205 // load and configure the DMP
syundo0730 0:8d2c753a96e7 206 //Serial.println(F("Initializing DMP..."));
syundo0730 0:8d2c753a96e7 207 pc.printf("Initializing DMP...\r\n");
syundo0730 0:8d2c753a96e7 208 devStatus = mpu.dmpInitialize();
syundo0730 0:8d2c753a96e7 209
syundo0730 0:8d2c753a96e7 210 // make sure it worked (returns 0 if so)
syundo0730 0:8d2c753a96e7 211 if (devStatus == 0) {
syundo0730 0:8d2c753a96e7 212 // turn on the DMP, now that it's ready
syundo0730 0:8d2c753a96e7 213 //Serial.println(F("Enabling DMP..."));
syundo0730 0:8d2c753a96e7 214 pc.printf("Enabling DMP...\r\n");
syundo0730 0:8d2c753a96e7 215 mpu.setDMPEnabled(true);
syundo0730 0:8d2c753a96e7 216
syundo0730 0:8d2c753a96e7 217 // enable Arduino interrupt detection
syundo0730 0:8d2c753a96e7 218 //Serial.println(F("Enabling interrupt detection..."));
syundo0730 0:8d2c753a96e7 219 pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\r\n");
syundo0730 0:8d2c753a96e7 220 checkpin.rise(&dmpDataReady);
syundo0730 0:8d2c753a96e7 221
syundo0730 0:8d2c753a96e7 222 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 223
syundo0730 0:8d2c753a96e7 224 // set our DMP Ready flag so the main loop() function knows it's okay to use it
syundo0730 0:8d2c753a96e7 225 //Serial.println(F("DMP ready! Waiting for first interrupt..."));
syundo0730 0:8d2c753a96e7 226 pc.printf("DMP ready! Waiting for first interrupt...\r\n");
syundo0730 0:8d2c753a96e7 227 dmpReady = true;
syundo0730 0:8d2c753a96e7 228
syundo0730 0:8d2c753a96e7 229 // get expected DMP packet size for later comparison
syundo0730 0:8d2c753a96e7 230 packetSize = mpu.dmpGetFIFOPacketSize();
syundo0730 0:8d2c753a96e7 231 } else {
syundo0730 0:8d2c753a96e7 232 // ERROR!
syundo0730 0:8d2c753a96e7 233 // 1 = initial memory load failed
syundo0730 0:8d2c753a96e7 234 // 2 = DMP configuration updates failed
syundo0730 0:8d2c753a96e7 235 // (if it's going to break, usually the code will be 1)
syundo0730 0:8d2c753a96e7 236 //Serial.print(F("DMP Initialization failed (code "));
syundo0730 0:8d2c753a96e7 237 //Serial.print(devStatus);
syundo0730 0:8d2c753a96e7 238 //Serial.println(F(")"));
syundo0730 0:8d2c753a96e7 239 pc.printf("DDMP Initialization failed (code ");
syundo0730 0:8d2c753a96e7 240 pc.printf("%d", devStatus);
syundo0730 0:8d2c753a96e7 241 pc.printf(")\r\n");
syundo0730 0:8d2c753a96e7 242 }
syundo0730 0:8d2c753a96e7 243
syundo0730 0:8d2c753a96e7 244 }
syundo0730 0:8d2c753a96e7 245
syundo0730 0:8d2c753a96e7 246
syundo0730 0:8d2c753a96e7 247
syundo0730 0:8d2c753a96e7 248 // ================================================================
syundo0730 0:8d2c753a96e7 249 // === MAIN PROGRAM LOOP ===
syundo0730 0:8d2c753a96e7 250 // ================================================================
syundo0730 0:8d2c753a96e7 251
syundo0730 0:8d2c753a96e7 252 void loop() {
syundo0730 0:8d2c753a96e7 253 // if programming failed, don't try to do anything
syundo0730 0:8d2c753a96e7 254 if (!dmpReady) return;
syundo0730 0:8d2c753a96e7 255
syundo0730 0:8d2c753a96e7 256 mpuInterrupt = true;
syundo0730 0:8d2c753a96e7 257 // wait for MPU interrupt or extra packet(s) available
syundo0730 0:8d2c753a96e7 258 while (!mpuInterrupt && fifoCount < packetSize) {
syundo0730 0:8d2c753a96e7 259 // other program behavior stuff here
syundo0730 0:8d2c753a96e7 260 // .
syundo0730 0:8d2c753a96e7 261 // .
syundo0730 0:8d2c753a96e7 262 // .
syundo0730 0:8d2c753a96e7 263 // if you are really paranoid you can frequently test in between other
syundo0730 0:8d2c753a96e7 264 // stuff to see if mpuInterrupt is true, and if so, "break;" from the
syundo0730 0:8d2c753a96e7 265 // while() loop to immediately process the MPU data
syundo0730 0:8d2c753a96e7 266 // .
syundo0730 0:8d2c753a96e7 267 // .
syundo0730 0:8d2c753a96e7 268 // .
syundo0730 0:8d2c753a96e7 269 }
syundo0730 0:8d2c753a96e7 270
syundo0730 0:8d2c753a96e7 271 // reset interrupt flag and get INT_STATUS byte
syundo0730 0:8d2c753a96e7 272 mpuInterrupt = false;
syundo0730 0:8d2c753a96e7 273 mpuIntStatus = mpu.getIntStatus();
syundo0730 0:8d2c753a96e7 274
syundo0730 0:8d2c753a96e7 275 // get current FIFO count
syundo0730 0:8d2c753a96e7 276 fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 277
syundo0730 0:8d2c753a96e7 278 // check for overflow (this should never happen unless our code is too inefficient)
syundo0730 0:8d2c753a96e7 279 if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
syundo0730 0:8d2c753a96e7 280 // reset so we can continue cleanly
syundo0730 0:8d2c753a96e7 281 mpu.resetFIFO();
syundo0730 0:8d2c753a96e7 282 //Serial.println(F("FIFO overflow!"));
syundo0730 0:8d2c753a96e7 283
syundo0730 0:8d2c753a96e7 284 // otherwise, check for DMP data ready interrupt (this should happen frequently)
syundo0730 0:8d2c753a96e7 285 } else if (mpuIntStatus & 0x02) {
syundo0730 0:8d2c753a96e7 286 // wait for correct available data length, should be a VERY short wait
syundo0730 0:8d2c753a96e7 287 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
syundo0730 0:8d2c753a96e7 288
syundo0730 0:8d2c753a96e7 289 // read a packet from FIFO
syundo0730 0:8d2c753a96e7 290 mpu.getFIFOBytes(fifoBuffer, packetSize);
syundo0730 0:8d2c753a96e7 291
syundo0730 0:8d2c753a96e7 292 // track FIFO count here in case there is > 1 packet available
syundo0730 0:8d2c753a96e7 293 // (this lets us immediately read more without waiting for an interrupt)
syundo0730 0:8d2c753a96e7 294 fifoCount -= packetSize;
syundo0730 0:8d2c753a96e7 295
syundo0730 0:8d2c753a96e7 296 #ifdef OUTPUT_READABLE_QUATERNION
syundo0730 0:8d2c753a96e7 297 // display quaternion values in easy matrix form: w x y z
syundo0730 0:8d2c753a96e7 298 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 299 //Serial.print("quat\t");
syundo0730 0:8d2c753a96e7 300 //Serial.print(q.w);
syundo0730 0:8d2c753a96e7 301 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 302 //Serial.print(q.x);
syundo0730 0:8d2c753a96e7 303 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 304 //Serial.print(q.y);
syundo0730 0:8d2c753a96e7 305 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 306 //Serial.println(q.z);
syundo0730 0:8d2c753a96e7 307 #endif
syundo0730 0:8d2c753a96e7 308
syundo0730 0:8d2c753a96e7 309 #ifdef OUTPUT_READABLE_EULER
syundo0730 0:8d2c753a96e7 310 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 311 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 312 mpu.dmpGetEuler(euler, &q);
syundo0730 0:8d2c753a96e7 313 //Serial.print("euler\t");
syundo0730 0:8d2c753a96e7 314 //Serial.print(euler[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 315 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 316 //Serial.print(euler[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 317 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 318 //Serial.println(euler[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 319 printf("euler\t");
syundo0730 0:8d2c753a96e7 320 printf("%f\t", euler[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 321 printf("%f\t", euler[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 322 printf("%f\t\r\n", euler[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 323 #endif
syundo0730 0:8d2c753a96e7 324
syundo0730 0:8d2c753a96e7 325 #ifdef OUTPUT_READABLE_YAWPITCHROLL
syundo0730 0:8d2c753a96e7 326 // display Euler angles in degrees
syundo0730 0:8d2c753a96e7 327 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 328 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 329 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
syundo0730 0:8d2c753a96e7 330 //Serial.print("ypr\t");
syundo0730 0:8d2c753a96e7 331 //Serial.print(ypr[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 332 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 333 //Serial.print(ypr[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 334 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 335 //Serial.println(ypr[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 336 printf("ypr\t");
syundo0730 0:8d2c753a96e7 337 printf("%f\t", ypr[0] * 180/M_PI);
syundo0730 0:8d2c753a96e7 338 printf("%f\t", ypr[1] * 180/M_PI);
syundo0730 0:8d2c753a96e7 339 printf("%f\t\r\n", ypr[2] * 180/M_PI);
syundo0730 0:8d2c753a96e7 340 #endif
syundo0730 0:8d2c753a96e7 341
syundo0730 0:8d2c753a96e7 342 #ifdef OUTPUT_READABLE_REALACCEL
syundo0730 0:8d2c753a96e7 343 // display real acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 344 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 345 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 346 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 347 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
syundo0730 0:8d2c753a96e7 348 //Serial.print("areal\t");
syundo0730 0:8d2c753a96e7 349 //Serial.print(aaReal.x);
syundo0730 0:8d2c753a96e7 350 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 351 //Serial.print(aaReal.y);
syundo0730 0:8d2c753a96e7 352 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 353 //Serial.println(aaReal.z);
syundo0730 0:8d2c753a96e7 354 #endif
syundo0730 0:8d2c753a96e7 355
syundo0730 0:8d2c753a96e7 356 #ifdef OUTPUT_READABLE_WORLDACCEL
syundo0730 0:8d2c753a96e7 357 // display initial world-frame acceleration, adjusted to remove gravity
syundo0730 0:8d2c753a96e7 358 // and rotated based on known orientation from quaternion
syundo0730 0:8d2c753a96e7 359 mpu.dmpGetQuaternion(&q, fifoBuffer);
syundo0730 0:8d2c753a96e7 360 mpu.dmpGetAccel(&aa, fifoBuffer);
syundo0730 0:8d2c753a96e7 361 mpu.dmpGetGravity(&gravity, &q);
syundo0730 0:8d2c753a96e7 362 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
syundo0730 0:8d2c753a96e7 363 //Serial.print("aworld\t");
syundo0730 0:8d2c753a96e7 364 //Serial.print(aaWorld.x);
syundo0730 0:8d2c753a96e7 365 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 366 //Serial.print(aaWorld.y);
syundo0730 0:8d2c753a96e7 367 //Serial.print("\t");
syundo0730 0:8d2c753a96e7 368 //Serial.println(aaWorld.z);
syundo0730 0:8d2c753a96e7 369 #endif
syundo0730 0:8d2c753a96e7 370
syundo0730 0:8d2c753a96e7 371 #ifdef OUTPUT_TEAPOT
syundo0730 0:8d2c753a96e7 372 // display quaternion values in InvenSense Teapot demo format:
syundo0730 0:8d2c753a96e7 373 teapotPacket[2] = fifoBuffer[0];
syundo0730 0:8d2c753a96e7 374 teapotPacket[3] = fifoBuffer[1];
syundo0730 0:8d2c753a96e7 375 teapotPacket[4] = fifoBuffer[4];
syundo0730 0:8d2c753a96e7 376 teapotPacket[5] = fifoBuffer[5];
syundo0730 0:8d2c753a96e7 377 teapotPacket[6] = fifoBuffer[8];
syundo0730 0:8d2c753a96e7 378 teapotPacket[7] = fifoBuffer[9];
syundo0730 0:8d2c753a96e7 379 teapotPacket[8] = fifoBuffer[12];
syundo0730 0:8d2c753a96e7 380 teapotPacket[9] = fifoBuffer[13];
syundo0730 0:8d2c753a96e7 381 //Serial.write(teapotPacket, 14);
syundo0730 0:8d2c753a96e7 382 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
syundo0730 0:8d2c753a96e7 383 #endif
syundo0730 0:8d2c753a96e7 384
syundo0730 0:8d2c753a96e7 385 // blink LED to indicate activity
syundo0730 0:8d2c753a96e7 386 //blinkState = !blinkState;
syundo0730 0:8d2c753a96e7 387 //digitalWrite(LED_PIN, blinkState);
syundo0730 0:8d2c753a96e7 388 led1 = !led1;
syundo0730 0:8d2c753a96e7 389 }
syundo0730 0:8d2c753a96e7 390 }