Components and supplies
Male/Female Jumper Wires
6 DOF Sensor - MPU6050
SG90 Micro-servo motor
Arduino Mega 2560
Male/Male Jumper Wires
Apps and platforms
Arduino IDE
Project description
Code
MPU6050_DMP6
arduino
Example file from MPU6050 library
1// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) 2// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net> 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/* ============================================ 21I2Cdev device library code is placed under the MIT license 22Copyright (c) 2012 Jeff Rowberg 23 24Permission is hereby granted, free of charge, to any person obtaining a copy 25of this software and associated documentation files (the "Software"), to deal 26in the Software without restriction, including without limitation the rights 27to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 28copies of the Software, and to permit persons to whom the Software is 29furnished to do so, subject to the following conditions: 30 31The above copyright notice and this permission notice shall be included in 32all copies or substantial portions of the Software. 33 34THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 35IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 36FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 37AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 38LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 39OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 40THE 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 61MPU6050 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) 122bool blinkState = false; 123 124// MPU control/status vars 125bool dmpReady = false; // set true if DMP init was successful 126uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 127uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 128uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 129uint16_t fifoCount; // count of all bytes currently in FIFO 130uint8_t fifoBuffer[64]; // FIFO storage buffer 131 132// orientation/motion vars 133Quaternion q; // [w, x, y, z] quaternion container 134VectorInt16 aa; // [x, y, z] accel sensor measurements 135VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 136VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 137VectorFloat gravity; // [x, y, z] gravity vector 138float euler[3]; // [psi, theta, phi] Euler angle container 139float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 140 141// packet structure for InvenSense teapot demo 142uint8_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 150volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 151void dmpDataReady() { 152 mpuInterrupt = true; 153} 154 155 156 157// ================================================================ 158// === INITIAL SETUP === 159// ================================================================ 160 161void 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("\ 192Send any character to begin DMP programming and demo: ")); 193 while (Serial.available() && Serial.read()); // empty buffer 194 while (!Serial.available()); // wait for data 195 while (Serial.available() && Serial.read()); // empty buffer again 196 197 // load and configure the DMP 198 Serial.println(F("Initializing DMP...")); 199 devStatus = mpu.dmpInitialize(); 200 201 // supply your own gyro offsets here, scaled for min sensitivity 202 mpu.setXGyroOffset(220); 203 mpu.setYGyroOffset(76); 204 mpu.setZGyroOffset(-85); 205 mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 206 207 // make sure it worked (returns 0 if so) 208 if (devStatus == 0) { 209 // turn on the DMP, now that it's ready 210 Serial.println(F("Enabling DMP...")); 211 mpu.setDMPEnabled(true); 212 213 // enable Arduino interrupt detection 214 Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 215 attachInterrupt(0, dmpDataReady, RISING); 216 mpuIntStatus = mpu.getIntStatus(); 217 218 // set our DMP Ready flag so the main loop() function knows it's okay to use it 219 Serial.println(F("DMP ready! Waiting for first interrupt...")); 220 dmpReady = true; 221 222 // get expected DMP packet size for later comparison 223 packetSize = mpu.dmpGetFIFOPacketSize(); 224 } else { 225 // ERROR! 226 // 1 = initial memory load failed 227 // 2 = DMP configuration updates failed 228 // (if it's going to break, usually the code will be 1) 229 Serial.print(F("DMP Initialization failed (code ")); 230 Serial.print(devStatus); 231 Serial.println(F(")")); 232 } 233 234 // configure LED for output 235 pinMode(LED_PIN, OUTPUT); 236} 237 238 239 240// ================================================================ 241// === MAIN PROGRAM LOOP === 242// ================================================================ 243 244void loop() { 245 // if programming failed, don't try to do anything 246 if (!dmpReady) return; 247 248 // wait for MPU interrupt or extra packet(s) available 249 while (!mpuInterrupt && fifoCount < packetSize) { 250 // other program behavior stuff here 251 // . 252 // . 253 // . 254 // if you are really paranoid you can frequently test in between other 255 // stuff to see if mpuInterrupt is true, and if so, "break;" from the 256 // while() loop to immediately process the MPU data 257 // . 258 // . 259 // . 260 } 261 262 // reset interrupt flag and get INT_STATUS byte 263 mpuInterrupt = false; 264 mpuIntStatus = mpu.getIntStatus(); 265 266 // get current FIFO count 267 fifoCount = mpu.getFIFOCount(); 268 269 // check for overflow (this should never happen unless our code is too inefficient) 270 if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 271 // reset so we can continue cleanly 272 mpu.resetFIFO(); 273 Serial.println(F("FIFO overflow!")); 274 275 // otherwise, check for DMP data ready interrupt (this should happen frequently) 276 } else if (mpuIntStatus & 0x02) { 277 // wait for correct available data length, should be a VERY short wait 278 while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 279 280 // read a packet from FIFO 281 mpu.getFIFOBytes(fifoBuffer, packetSize); 282 283 // track FIFO count here in case there is > 1 packet available 284 // (this lets us immediately read more without waiting for an interrupt) 285 fifoCount -= packetSize; 286 287 #ifdef OUTPUT_READABLE_QUATERNION 288 // display quaternion values in easy matrix form: w x y z 289 mpu.dmpGetQuaternion(&q, fifoBuffer); 290 Serial.print("quat\ "); 291 Serial.print(q.w); 292 Serial.print("\ "); 293 Serial.print(q.x); 294 Serial.print("\ "); 295 Serial.print(q.y); 296 Serial.print("\ "); 297 Serial.println(q.z); 298 #endif 299 300 #ifdef OUTPUT_READABLE_EULER 301 // display Euler angles in degrees 302 mpu.dmpGetQuaternion(&q, fifoBuffer); 303 mpu.dmpGetEuler(euler, &q); 304 Serial.print("euler\ "); 305 Serial.print(euler[0] * 180/M_PI); 306 Serial.print("\ "); 307 Serial.print(euler[1] * 180/M_PI); 308 Serial.print("\ "); 309 Serial.println(euler[2] * 180/M_PI); 310 #endif 311 312 #ifdef OUTPUT_READABLE_YAWPITCHROLL 313 // display Euler angles in degrees 314 mpu.dmpGetQuaternion(&q, fifoBuffer); 315 mpu.dmpGetGravity(&gravity, &q); 316 mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 317 Serial.print("ypr\ "); 318 Serial.print(ypr[0] * 180/M_PI); 319 Serial.print("\ "); 320 Serial.print(ypr[1] * 180/M_PI); 321 Serial.print("\ "); 322 Serial.println(ypr[2] * 180/M_PI); 323 #endif 324 325 #ifdef OUTPUT_READABLE_REALACCEL 326 // display real acceleration, adjusted to remove gravity 327 mpu.dmpGetQuaternion(&q, fifoBuffer); 328 mpu.dmpGetAccel(&aa, fifoBuffer); 329 mpu.dmpGetGravity(&gravity, &q); 330 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 331 Serial.print("areal\ "); 332 Serial.print(aaReal.x); 333 Serial.print("\ "); 334 Serial.print(aaReal.y); 335 Serial.print("\ "); 336 Serial.println(aaReal.z); 337 #endif 338 339 #ifdef OUTPUT_READABLE_WORLDACCEL 340 // display initial world-frame acceleration, adjusted to remove gravity 341 // and rotated based on known orientation from quaternion 342 mpu.dmpGetQuaternion(&q, fifoBuffer); 343 mpu.dmpGetAccel(&aa, fifoBuffer); 344 mpu.dmpGetGravity(&gravity, &q); 345 mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 346 mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); 347 Serial.print("aworld\ "); 348 Serial.print(aaWorld.x); 349 Serial.print("\ "); 350 Serial.print(aaWorld.y); 351 Serial.print("\ "); 352 Serial.println(aaWorld.z); 353 #endif 354 355 #ifdef OUTPUT_TEAPOT 356 // display quaternion values in InvenSense Teapot demo format: 357 teapotPacket[2] = fifoBuffer[0]; 358 teapotPacket[3] = fifoBuffer[1]; 359 teapotPacket[4] = fifoBuffer[4]; 360 teapotPacket[5] = fifoBuffer[5]; 361 teapotPacket[6] = fifoBuffer[8]; 362 teapotPacket[7] = fifoBuffer[9]; 363 teapotPacket[8] = fifoBuffer[12]; 364 teapotPacket[9] = fifoBuffer[13]; 365 Serial.write(teapotPacket, 14); 366 teapotPacket[11]++; // packetCount, loops at 0xFF on purpose 367 #endif 368 369 // blink LED to indicate activity 370 blinkState = !blinkState; 371 digitalWrite(LED_PIN, blinkState); 372 } 373}
Downloadable files
Schematic
Schematic
Schematic
Schematic
Comments
Only logged in users can leave comments
Simple 2-axis Servo Robotic Arm controlled by MPU-6050 | Arduino Project Hub