Guest User

Untitled

a guest
Feb 7th, 2014
335
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 16.76 KB | None | 0 0
  1. // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
  2. // 6/21/2012 by Jeff Rowberg <[email protected]>
  3. // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
  4. //
  5. // Changelog:
  6. //      2013-05-08 - added seamless Fastwire support
  7. //                 - added note about gyro calibration
  8. //      2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
  9. //      2012-06-20 - improved FIFO overflow handling and simplified read process
  10. //      2012-06-19 - completely rearranged DMP initialization code and simplification
  11. //      2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
  12. //      2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
  13. //      2012-06-05 - add gravity-compensated initial reference frame acceleration output
  14. //                 - add 3D math helper file to DMP6 example sketch
  15. //                 - add Euler output and Yaw/Pitch/Roll output formats
  16. //      2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
  17. //      2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
  18. //      2012-05-30 - basic DMP initialization working
  19.  
  20. /* ============================================
  21. I2Cdev device library code is placed under the MIT license
  22. Copyright (c) 2012 Jeff Rowberg
  23.  
  24. Permission is hereby granted, free of charge, to any person obtaining a copy
  25. of this software and associated documentation files (the "Software"), to deal
  26. in the Software without restriction, including without limitation the rights
  27. to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  28. copies of the Software, and to permit persons to whom the Software is
  29. furnished to do so, subject to the following conditions:
  30.  
  31. The above copyright notice and this permission notice shall be included in
  32. all copies or substantial portions of the Software.
  33.  
  34. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  35. IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  36. FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  37. AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  38. LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  39. OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  40. THE SOFTWARE.
  41. ===============================================
  42. */
  43.  
  44. // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
  45. // for both classes must be in the include path of your project
  46. #include "I2Cdev.h"
  47.  
  48. #include "MPU6050_6Axis_MotionApps20.h"
  49. //#include "MPU6050.h" // not necessary if using MotionApps include file
  50.  
  51. // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
  52. // is used in I2Cdev.h
  53. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  54.     #include "Wire.h"
  55. #endif
  56.  
  57. // class default I2C address is 0x68
  58. // specific I2C addresses may be passed as a parameter here
  59. // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
  60. // AD0 high = 0x69
  61. MPU6050 mpu;
  62. //MPU6050 mpu(0x69); // <-- use for AD0 high
  63.  
  64. /* =========================================================================
  65.    NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
  66.    depends on the MPU-6050's INT pin being connected to the Arduino's
  67.    external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
  68.    digital I/O pin 2.
  69.  * ========================================================================= */
  70.  
  71. /* =========================================================================
  72.    NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
  73.    when using Serial.write(buf, len). The Teapot output uses this method.
  74.    The solution requires a modification to the Arduino USBAPI.h file, which
  75.    is fortunately simple, but annoying. This will be fixed in the next IDE
  76.    release. For more info, see these links:
  77.  
  78.    http://arduino.cc/forum/index.php/topic,109987.0.html
  79.    http://code.google.com/p/arduino/issues/detail?id=958
  80.  * ========================================================================= */
  81.  
  82.  
  83.  
  84. // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
  85. // quaternion components in a [w, x, y, z] format (not best for parsing
  86. // on a remote host such as Processing or something though)
  87. //#define OUTPUT_READABLE_QUATERNION
  88.  
  89. // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
  90. // (in degrees) calculated from the quaternions coming from the FIFO.
  91. // Note that Euler angles suffer from gimbal lock (for more info, see
  92. // http://en.wikipedia.org/wiki/Gimbal_lock)
  93. //#define OUTPUT_READABLE_EULER
  94.  
  95. // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
  96. // pitch/roll angles (in degrees) calculated from the quaternions coming
  97. // from the FIFO. Note this also requires gravity vector calculations.
  98. // Also note that yaw/pitch/roll angles suffer from gimbal lock (for
  99. // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
  100. #define OUTPUT_READABLE_YAWPITCHROLL
  101.  
  102. // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
  103. // components with gravity removed. This acceleration reference frame is
  104. // not compensated for orientation, so +X is always +X according to the
  105. // sensor, just without the effects of gravity. If you want acceleration
  106. // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
  107. //#define OUTPUT_READABLE_REALACCEL
  108.  
  109. // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
  110. // components with gravity removed and adjusted for the world frame of
  111. // reference (yaw is relative to initial orientation, since no magnetometer
  112. // is present in this case). Could be quite handy in some cases.
  113. //#define OUTPUT_READABLE_WORLDACCEL
  114.  
  115. // uncomment "OUTPUT_TEAPOT" if you want output that matches the
  116. // format used for the InvenSense teapot demo
  117. //#define OUTPUT_TEAPOT
  118.  
  119.  
  120.  
  121. #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
  122. bool blinkState = false;
  123.  
  124. // MPU control/status vars
  125. bool dmpReady = false;  // set true if DMP init was successful
  126. uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
  127. uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
  128. uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
  129. uint16_t fifoCount;     // count of all bytes currently in FIFO
  130. uint8_t fifoBuffer[64]; // FIFO storage buffer
  131.  
  132. // orientation/motion vars
  133. Quaternion q;           // [w, x, y, z]         quaternion container
  134. VectorInt16 aa;         // [x, y, z]            accel sensor measurements
  135. VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
  136. VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
  137. VectorFloat gravity;    // [x, y, z]            gravity vector
  138. float euler[3];         // [psi, theta, phi]    Euler angle container
  139. float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
  140.  
  141. // packet structure for InvenSense teapot demo
  142. uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
  143.  
  144.  
  145.  
  146. // ================================================================
  147. // ===               INTERRUPT DETECTION ROUTINE                ===
  148. // ================================================================
  149.  
  150. volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
  151. void dmpDataReady() {
  152.     mpuInterrupt = true;
  153. }
  154.  
  155.  
  156.  
  157. // ================================================================
  158. // ===                      INITIAL SETUP                       ===
  159. // ================================================================
  160.  
  161. void setup() {
  162.     // join I2C bus (I2Cdev library doesn't do this automatically)
  163.     #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  164.         Wire.begin();
  165.         TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
  166.     #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  167.         Fastwire::setup(400, true);
  168.     #endif
  169.  
  170.     // initialize serial communication
  171.     // (115200 chosen because it is required for Teapot Demo output, but it's
  172.     // really up to you depending on your project)
  173.     Serial.begin(115200);
  174.     while (!Serial); // wait for Leonardo enumeration, others continue immediately
  175.  
  176.     // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
  177.     // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
  178.     // the baud timing being too misaligned with processor ticks. You must use
  179.     // 38400 or slower in these cases, or use some kind of external separate
  180.     // crystal solution for the UART timer.
  181.  
  182.     // initialize device
  183.     Serial.println(F("Initializing I2C devices..."));
  184.     mpu.initialize();
  185.  
  186.     // verify connection
  187.     Serial.println(F("Testing device connections..."));
  188.     Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
  189.  
  190.     // wait for ready
  191.     Serial.println(F("\nSend any character to begin DMP programming and demo: "));
  192.     while (Serial.available() && Serial.read()); // empty buffer
  193.     while (!Serial.available());                 // wait for data
  194.     while (Serial.available() && Serial.read()); // empty buffer again
  195.  
  196.     // load and configure the DMP
  197.     Serial.println(F("Initializing DMP..."));
  198.     devStatus = mpu.dmpInitialize();
  199.  
  200.     // supply your own gyro offsets here, scaled for min sensitivity
  201.     mpu.setXGyroOffset(220);
  202.     mpu.setYGyroOffset(76);
  203.     mpu.setZGyroOffset(-85);
  204.     mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
  205. //     MPU 1
  206. //    mpu.setXGyroOffset(90);
  207. //    mpu.setYGyroOffset(5);
  208. //    mpu.setZGyroOffset(-17);
  209.    
  210. //    mpu.setXAccelOffset(-3595);
  211. //    mpu.setYAccelOffset(-3563);
  212. //    mpu.setZAccelOffset(1553);
  213.  
  214. // MPU 2
  215. /*Calculated offsets:
  216.  
  217.  x accel: 588
  218.  
  219.  y accel: -6802
  220.  
  221.  z accel: 1577
  222.  
  223.  x gyro: 170
  224.  
  225.  y gyro: -4
  226.  
  227.  z gyro: 8
  228. */
  229.  
  230.  
  231.     // make sure it worked (returns 0 if so)
  232.     if (devStatus == 0) {
  233.         // turn on the DMP, now that it's ready
  234.         Serial.println(F("Enabling DMP..."));
  235.         mpu.setDMPEnabled(true);
  236.  
  237.         // enable Arduino interrupt detection
  238.         Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
  239.         attachInterrupt(0, dmpDataReady, RISING);
  240.         mpuIntStatus = mpu.getIntStatus();
  241.  
  242.         // set our DMP Ready flag so the main loop() function knows it's okay to use it
  243.         Serial.println(F("DMP ready! Waiting for first interrupt..."));
  244.         dmpReady = true;
  245.  
  246.         // get expected DMP packet size for later comparison
  247.         packetSize = mpu.dmpGetFIFOPacketSize();
  248.     } else {
  249.         // ERROR!
  250.         // 1 = initial memory load failed
  251.         // 2 = DMP configuration updates failed
  252.         // (if it's going to break, usually the code will be 1)
  253.         Serial.print(F("DMP Initialization failed (code "));
  254.         Serial.print(devStatus);
  255.         Serial.println(F(")"));
  256.     }
  257.  
  258.     // configure LED for output
  259.     pinMode(LED_PIN, OUTPUT);
  260. }
  261.  
  262.  
  263. int lastNow, now;
  264.  
  265.  
  266. // ================================================================
  267. // ===                    MAIN PROGRAM LOOP                     ===
  268. // ================================================================
  269.  
  270. void loop() {
  271.  
  272.   now = millis();
  273.   int dt = now - lastNow;
  274.   lastNow = now;
  275.     // if programming failed, don't try to do anything
  276.     if (!dmpReady) return;
  277.  
  278.     // wait for MPU interrupt or extra packet(s) available
  279.     while (!mpuInterrupt && fifoCount < packetSize) {
  280.       Serial.println("Waiting...");
  281.         // other program behavior stuff here
  282.         // .
  283.         // .
  284.         // .
  285.         // if you are really paranoid you can frequently test in between other
  286.         // stuff to see if mpuInterrupt is true, and if so, "break;" from the
  287.         // while() loop to immediately process the MPU data
  288.         // .
  289.         // .
  290.         // .
  291.     }
  292.  
  293.     // reset interrupt flag and get INT_STATUS byte
  294.     mpuInterrupt = false;
  295.     mpuIntStatus = mpu.getIntStatus();
  296.  
  297.     // get current FIFO count
  298.     fifoCount = mpu.getFIFOCount();
  299.  
  300.     // check for overflow (this should never happen unless our code is too inefficient)
  301.     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
  302.         // reset so we can continue cleanly
  303.         mpu.resetFIFO();
  304. //        fifoCount = 0;
  305.         Serial.println(F("FIFO overflow!"));
  306.  
  307.     // otherwise, check for DMP data ready interrupt (this should happen frequently)
  308.     }else
  309.     if (mpuIntStatus & 0x02) {
  310.         // wait for correct available data length, should be a VERY short wait
  311.         while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
  312.  
  313.         // read a packet from FIFO
  314.         mpu.getFIFOBytes(fifoBuffer, packetSize);
  315.        
  316.         // track FIFO count here in case there is > 1 packet available
  317.         // (this lets us immediately read more without waiting for an interrupt)
  318.         fifoCount -= packetSize;
  319.  
  320.         #ifdef OUTPUT_READABLE_QUATERNION
  321.             // display quaternion values in easy matrix form: w x y z
  322.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  323.             Serial.print("quat\t");
  324.             Serial.print(q.w);
  325.             Serial.print("\t");
  326.             Serial.print(q.x);
  327.             Serial.print("\t");
  328.             Serial.print(q.y);
  329.             Serial.print("\t");
  330.             Serial.println(q.z);
  331.         #endif
  332.  
  333.         #ifdef OUTPUT_READABLE_EULER
  334.             // display Euler angles in degrees
  335.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  336.             mpu.dmpGetEuler(euler, &q);
  337.             Serial.print("euler\t");
  338.             Serial.print(euler[0] * 180/M_PI);
  339.             Serial.print("\t");
  340.             Serial.print(euler[1] * 180/M_PI);
  341.             Serial.print("\t");
  342.             Serial.println(euler[2] * 180/M_PI);
  343.         #endif
  344.  
  345.         #ifdef OUTPUT_READABLE_YAWPITCHROLL
  346.             // display Euler angles in degrees
  347.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  348.             mpu.dmpGetGravity(&gravity, &q);
  349.             mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  350.             if (ypr[2] * 180/M_PI > 32 || ypr[2] * 180/M_PI < 27){
  351.             Serial.print("ypr\t");
  352.             Serial.print(ypr[0] * 180/M_PI);
  353.             Serial.print("\t");
  354.             Serial.print(ypr[1] * 180/M_PI);
  355.             Serial.print("\t");
  356.             Serial.print(ypr[2] * 180/M_PI);
  357.             Serial.print("\t");
  358.             Serial.println(dt);
  359.             }
  360.            
  361.             //Serial.print("ypr\t");
  362.             //Serial.print(gravity.x);
  363.             //Serial.print("\t");
  364.             //Serial.print(gravity.y);
  365.             //Serial.print("\t");
  366.             //Serial.println(gravity.z);
  367.         #endif
  368.  
  369.         #ifdef OUTPUT_READABLE_REALACCEL
  370.             // display real acceleration, adjusted to remove gravity
  371.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  372.             mpu.dmpGetAccel(&aa, fifoBuffer);
  373.             mpu.dmpGetGravity(&gravity, &q);
  374.             mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  375.             Serial.print("areal\t");
  376.             Serial.print(aaReal.x);
  377.             Serial.print("\t");
  378.             Serial.print(aaReal.y);
  379.             Serial.print("\t");
  380.             Serial.println(aaReal.z);
  381.         #endif
  382.  
  383.         #ifdef OUTPUT_READABLE_WORLDACCEL
  384.             // display initial world-frame acceleration, adjusted to remove gravity
  385.             // and rotated based on known orientation from quaternion
  386.             mpu.dmpGetQuaternion(&q, fifoBuffer);
  387.             mpu.dmpGetAccel(&aa, fifoBuffer);
  388.             mpu.dmpGetGravity(&gravity, &q);
  389.             mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
  390.             mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
  391.             Serial.print("aworld\t");
  392.             Serial.print(aaWorld.x);
  393.             Serial.print("\t");
  394.             Serial.print(aaWorld.y);
  395.             Serial.print("\t");
  396.             Serial.println(aaWorld.z);
  397.         #endif
  398.    
  399.         #ifdef OUTPUT_TEAPOT
  400.             // display quaternion values in InvenSense Teapot demo format:
  401.             teapotPacket[2] = fifoBuffer[0];
  402.             teapotPacket[3] = fifoBuffer[1];
  403.             teapotPacket[4] = fifoBuffer[4];
  404.             teapotPacket[5] = fifoBuffer[5];
  405.             teapotPacket[6] = fifoBuffer[8];
  406.             teapotPacket[7] = fifoBuffer[9];
  407.             teapotPacket[8] = fifoBuffer[12];
  408.             teapotPacket[9] = fifoBuffer[13];
  409.             Serial.write(teapotPacket, 14);
  410.             teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
  411.         #endif
  412.  
  413.         // blink LED to indicate activity
  414.         blinkState = !blinkState;
  415.         digitalWrite(LED_PIN, blinkState);
  416.     }
  417.    
  418.     delay(6);
  419. }
Advertisement
Add Comment
Please, Sign In to add comment