how to connect two mpu6050 to arduino uno

[reut10]

Hi! I want to connect my arduino uno to two mpu 6050 in order to learn about the relative angles and velocity, there is some ready program for this? how to connect the 2 mpu 6050 ? thank you, reut :)

[Ernest314]

You can pull one of the AD0 pins up and one of them down; then you can address the two MPUs with different I2C addresses.

[reut10]

thank!!! but, how I can address the two MPUs with different I2C addresses?

[udawat]

Create two MPU6050 objects that can be called separately. See below:

MPU6050 accelgyroIC1(0x68);
MPU6050 accelgyroIC2(0x69); // <-- use for second MPU6050 device where AD0 must be high (3.3V)

Then, initialize both the objects with:

accelgyroIC1.initialize();
accelgyroIC2.initialize();

You may test the connection in the same way. Then call the getMotion6() function twice.

[loowiz]

Hi!

I'm doing the same!

See the modifications below:

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class // 10/7/2011 by Jeff Rowberg jeff@rowberg.net // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: // 2011-10-07 - initial release

/* ============================================ I2Cdev device library code is placed under the MIT license Copyright (c) 2011 Jeff Rowberg

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

THE SOFTWARE.

*/

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation // is used in I2Cdev.h

include "Wire.h"

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files // for both classes must be in the include path of your project

include "I2Cdev.h"

include "MPU6050.h"

// class default I2C address is 0x68 // specific I2C addresses may be passed as a parameter here // AD0 low = 0x68 (default for InvenSense evaluation board) // AD0 high = 0x69 //MPU6050 accelgyro; MPU6050 accelgyroIC1(0x68); MPU6050 accelgyroIC2(0x69);

int16_t ax1, ay1, az1; int16_t gx1, gy1, gz1;

int16_t ax2, ay2, az2; int16_t gx2, gy2, gz2;

define LED_PIN 13

bool blinkState = false;

void setup() { // join I2C bus (I2Cdev library doesn't do this automatically) Wire.begin();

// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz,

but // it's really up to you depending on your project) Serial.begin(38400);

// initialize device
Serial.println("Initializing I2C devices...");
//accelgyro.initialize();
accelgyroIC1.initialize();
accelgyroIC2.initialize();

// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyroIC1.testConnection() ? "MPU6050 #1 connection

successful" : "MPU6050 connection failed"); Serial.println(accelgyroIC2.testConnection() ? "MPU6050 #2 connection successful" : "MPU6050 connection failed");

// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);

}

void loop() { // read raw accel/gyro measurements from device accelgyroIC1.getMotion6(&ax1, &ay1, &az1, &gx1, &gy1, &gz1); accelgyroIC2.getMotion6(&ax2, &ay2, &az2, &gx2, &gy2, &gz2);

// these methods (and a few others) are also available
//accelgyro.getAcceleration(&ax, &ay, &az);
//accelgyro.getRotation(&gx, &gy, &gz);

// display tab-separated accel/gyro x/y/z values
Serial.print("MPU1:\t");
Serial.print(ax1); Serial.print("\t");
Serial.print(ay1); Serial.print("\t");
Serial.print(az1); Serial.print("\t");
Serial.print(gx1); Serial.print("\t");
Serial.print(gy1); Serial.print("\t");
Serial.println(gz1);

// display tab-separated accel/gyro x/y/z values
Serial.print("MPU2:\t");
Serial.print(ax2); Serial.print("\t");
Serial.print(ay2); Serial.print("\t");
Serial.print(az2); Serial.print("\t");
Serial.print(gx2); Serial.print("\t");
Serial.print(gy2); Serial.print("\t");
Serial.println(gz2);

// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);

}

2014-03-20 8:21 GMT-03:00 udawat notifications@github.com:

Create two MPU6050 objects that can be called separately. See below:

MPU6050 accelgyroIC2(0x69); // <-- use for second MPU6050 device where AD0
must be high (3.3V)

Then, initialize both the objects with:

``` accelgyroIC1.initialize();
 accelgyroIC2.initialize();

You may test the connection in the same way. Then call the getMotion6()
function twice.

--
Reply to this email directly or view it on GitHub<https://github.com/jrowberg/i2cdevlib/issues/63#issuecomment-38156087>
.

[reut10]

thanks!!

looowiz, in your project you gen the two angles for example and not the relative one; do you know how to get it?

there is graphical view in order to see the current location/relative angles and velocity of the two mpu?

[reut10]

loowiz could you please add your circuit scheme? I found here: http://forum.arduino.cc/index.php?topic=118937.0 three ways to connect the two MPU6050 to the Arduino Uno but non of them works..

Thank you

[loowiz]

Hi Reut10!!

To correct your error I recommend you to download the original sketch files on https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050 Then you can make the changes using the code I sent! I think you forgot to put the libraries on the same directory of your sketch.

For the hardware, Im using connections similar to the Erguezga post ( http://forum.arduino.cc/index.php?PHPSESSID=t6lmv7i431eqeaf05ui619q2h6&topic=118937.msg1368958#msg1368958). The difference is that Im not using the orange wires (Int outputs) and I connected MPU#1s AD0 to GND and MPU#2s AD0 to VCC. After this, one MPU take the address 0x68 and the other take 0x69. Im using an Arduino Mega board, but its the same code!

I'm curious, what purpose of your project?

2014-03-23 7:25 GMT-03:00 reut10 notifications@github.com:

loowiz could you please add your circuit scheme? I found here: http://forum.arduino.cc/index.php?topic=118937.0 three ways to connect the two MPU6050 to the Arduino Uno but non of them works..

Thank you

Reply to this email directly or view it on GitHubhttps://github.com/jrowberg/i2cdevlib/issues/63#issuecomment-38378705 .

[reut10]

Thank you!! I will try this soon and let you know how it worked out. We are trying to calculate the wrist range of motion by the relative velocity and angle for bio-feedback rehabilitation, also we want to visualize this data on a graph. Did you calibrate the raw data in order to get Physical values?

[reut10]

Hi! we did as loowiz said, now we can read data from each mpu 6050 in 68/69. when we try to connect the 2 mpu 6050 together we can read just one of then. the connection we did is: A4- SDA1-SDA2 A5-SCL1-SCL2 3.3-VDD1-VIO1-GND2 GND-VDD2-VIO2-GND1

and use loowiz code...but it's doesn't work. We try to use I2C scanner and we can't read the 2 mpu 6050, but for each we can read. what's wrong?

Thanks again!

[udawat]

@reut10 Power connections seem to be improper. Try the following configuration: 3.3V -> VDD1-VDD2-VIO1-VIO2 GND -> GND1-GND2

[reut10]

I can read data from the two IMU!!!!! thanks!!!! @udawat you right!!!!!

Now, I have to get the date from the two MPU6050 after complementary filter. (I use processing in order to see a graph of the movement, for one mpu I get a roll and pitch angle) do you know how to do it?

Thank you! Reut

[circuitmagic]

You have to connect VCC - Arduino 5v GND - Arduino GND SCL - A5 SDA - A4

and here is some example video and arduino code.

https://www.youtube.com/watch?v=D5HY_tjW1lc https://www.youtube.com/watch?v=EbIGiKwFj3c

[54272115]

@reut10 How does your circuit looks? I'm totally confused with connecting 2 MPU6050 to a Arduino Uno R3 board

[ranqingfa]

@54272115 Do you have the 2 MPU6050 in different address ？

[reut10]

we connect the 2 MPU 6050 in parallel and they have different address

On Mon, Sep 15, 2014 at 4:22 AM, RudeBoy notifications@github.com wrote:

@54272115 https://github.com/54272115 Do you have the 2 MPU6050 in different address ？

— Reply to this email directly or view it on GitHub https://github.com/jrowberg/i2cdevlib/issues/63#issuecomment-55546098.

[tsimon57]

Be careful if you are using MotionApps. There are hard coded I2C addresses in the current version for some of the initialization. Search for 0x68 in the code....

[reut10]

Thanks!! I finished my project :)

On Tue, Sep 30, 2014 at 2:09 AM, tsimon57 notifications@github.com wrote:

Be careful if you are using MotionApps. There are hard coded I2C addresses in the current version for some of the initialization. Search for 0x68 in the code....

— Reply to this email directly or view it on GitHub https://github.com/jrowberg/i2cdevlib/issues/63#issuecomment-57244285.

[serenaricci]

Hello everybody, We should connect two mpu6050 to an Arduino mega but we don't know how to connect them physically. We know that we have to turn the i2c adress from 0x68 to 0x69 in order to have 2 different adress, one for each sensor. Someone says that it must connect them in parallel but other thinks that it is usefull to set two digital pins high or low to "swich on/off" a sensor in order to read information alternated. moreover we would like to read quarternion or euler data but now we are only able to receive raw data. May anyone help us? thanks

[jorgediosdado]

Hi, I am also trying to read values from two MPU6050. I just started looking into this, and for what I've read, raw values can be read from both devices.

My question is, is it possible to read values computed by DMP for two MPU6050?

Thanks!

[eadf]

Yes, it is possible to connect more than one MPU6050 to an Arduino. I'm currently testing an extended example of MPU6050_DMP.ino that can connect to any number of accelerometers. The only limit is the number of interrupt pins.

I'll upload a temporary repository here: https://github.com/eadf/MPU6050_DMP6_Multiple Feedback is welcome.

[jorgediosdado]

Cool, It looks really good. What is the correct hardware setup? Do I need to do something in order to change I2C addresses? Any idea of what frequency can I get working with two IMUs? I am relatively new to I2C and this sensor, so any help is appreciated!

[eadf]

With the MPU6050_DMP6_Multiple example you will always use the MPU6050_ADDRESS_AD0_HIGH address to talk to the MPU6050s. The wrapper code in the example will set the AD0 high on the device it wants to talk to (all the others will have AD0 low)

At 100Hz FIFO a 8Mhz pro mini is just about able to printout pitch and roll of two devices. So I use to lower the FIFO frequency with MPU6050_DMP_FIFO_RATE_DIVISOR (a feature only found in the develop branch of i2cdevlib).

I have not tested the max speed yet. But I imagine a Due or esp8266 should be able to handle quite high FIFO rates.

Regarding the wiring.. you just setup the devices with the i2c pins in parallel as usual. AD0 and INT needs to be connected to separate Arduino pins. Look at the README.md

[jorgediosdado]

I succesfully read values from the two imus (pitch and roll). Everything is good for some time and then it stops. Any suggestion?

[eadf]

MPU6050_DMP_FIFO_RATE_DIVISOR could be set too low relative to your CPU speed and Serial transmission speed (it's set in MPU6050_Wrapper.h)

Remove all frequent and unnecessary Serial.print():s. In the demo I printout the measured FIFO speed in Hz for each sample. Remove that.

In MPU6050_Array::select(uint8_t device) there is a delay(1) that I commented out for some reason.. Maybe the IMUs needs some time after AD0 is switched. Try to re-add that delay and try different delay values.

[eadf]

Hm, I just tested with delay(1) and MPU6050_DMP_FIFO_RATE_DIVISOR=3 it ran for some 30 minutes on a Pro Mini 8MHz, then it stopped. I'll have to investigate this some more.

[jorgediosdado]

I am also changing delay time and MPU6050_DMP_FIFO_RATE_DIVISOR time, but no luck on getting it to work for more than half a minute or so.

[eadf]

Are you using the develop branch of i2cdevlib?

Let's move the discussion to the MPU6050_DMP6_Multiple repository. Open an issue there and we'll try to figure this out.

[felipeLvalero]

@jorgediosdado @eadf Hi, I'm pretty new in this arduino World, I'm reading your code and can see how you read the accelerometer but, how to read the Gyro in the two MPUs?

thanks in advance.

[jwaluyo]

@loowiz Hi, I'm beginner in this thing can you help me please. i've tried to modification I2C code you sent up and put the libraries on the same directory of sketch but still have trouble when i compiled

[jwaluyo]

@reut10 can you help me for my trouble please? i'm confusing with the code, so many errors in my sketch that i can't understand...

[Haro193]

Hi, I want to connect my arduino mega to three or more mpu 6050, is there some ready program for this? how to connect the 3 or more mpu 6050?

thank you, LucasHaro.

[piroozn]

You can connect two MPU6050 on the I2C buss without any additional circuit. If you want more than 2 then you have to use a GPIO to control each MPU6050 AD) and selectively set it to 1 on the MPU6050 that you want selected and address that device with LSB set to 1. Please refer to its data sheet and simply select the device you want by setting GPIO to 1 on the selected device and that would be enable on the I2C when your address LSB is 1. Your control processor most likely needs to be higher performance to be able to handle such configuration since you are going to get lots of data. You may want to switch to a higher performance Arduino to be able to successfully use it in this configuration. Multiple core may be suitable for this configuration.

[hamiderza]

hi, i want to connect three MPU6050 with and an arduino mega or uno. i know that using multiplexer such as 74hct4067 is an solution for it, is there some ready program to run it? can you help me about circuit and connecting two MPU6050 with each others ?

[ridwankabir]

I think I might have figured out a way to use Jeff Rowberg's example of using DMP in two MPU6050s.

I am using an Arduino NANO and for the record it has INTERRUPTS attached to two pins Digital I/O pins 2 and 3 and has on board 5V and 3.3V power supply.

I've used pin 2 for MPU(0x68) with AD0 set to LOW and pin 3 for MPU(0x69) with AD0 set to HIGH. I also connected a 2.2k pull-up resistors with each SCL and SDA on the MPU(0x69).

I repeated the same code for each of the MPUs with separate variables.

I found out that supplying Arduino's 5V to AD0 on MPU(0x69) caused the program to stop writing on the serial monitor after a while. But as soon as I connected AD0 to 3.3V on the Arduino............. voila!!!! problem gone. I have been successful in collecting data from both the MPUs and also a flex sensor for over an hour now without any problem.

I hope this helps. Thank you.

Ridwan Kabir

[ianflowe]

Hi Ridwan kabir I'm working on a project to make a two plane balancer to dynamically balance small otto engine cranks. I decided to use two mpu6050 to collect vibration data one on each plane and a lm393 photo resistor to collect rotation in rpm. I also have a led 20 x 4 screen connected i2c. When I connect one of the mpu6050's the screen goes off, is this due to not having any pull up resisters in circuit ? i will get some resistor and try. could you post your code so I can use a code that works to assess my setup. I was wondering whether to connect the second mpu6050 to the first using the slave i2c by connecting the xda & xcl of the second to the sda & scl of the first is this something that anyone has tried.

Regards Ian Lowe

[ridwankabir]

Hi Ian Lowe,

Have you specified any address for the led? Like 0x3f or 0x27 ? You may check this link for details on led connection and stuff

https://github.com/zegreatclan/AssettoCorsaTools/wiki/Arduino-20x4-I2C-LCD

As the SDA and SCL lines are common the devices must have separate addresses. For example , two mpu 6050s need to have addresses 0x68( AD0 pin set to GND ) and 0x69 (AD0 pin set to 3.3 v) for them communicate via i2c simultaneously and also separate interrupt lines are required. Make sure not to apply 5v on VCC of the MPU with address 0x69 as it has already been powered by 3.3 v on the AD0 pin. At first, I used the pullup resistors on the second MPU but then I removed them. It works just fine in both cases. I have not tried using one MPU as a slave to the other. So I am not sure about tha XDA,XCL connection part.

[Maruthi-97]

Hai everyone I am trying to use 5 mpu6050 units (GY 521 boards) and the task is all should be read simultaneously. Have anyone used like this earlier? I have used two units with Arduino Mega 2560. Please suggest me the best microcontroller to drive all units along with Kalman code.

[franclab1887]

Hello everyone, I am currently trying to use two MPU6050 on a Arduino UNO and I have the overflow issue like most of the people. You can find my code down below, it only work for some values and the rest are just overflow or 0 0 90 degrees that can't be right. @ridwankabir I tried to double everything as you said and yet it doesn't work if you can help me please? for the connections the gyro one : Vcc==>5v, gnd==>gnd, SCL==> A5, SCA ==> A4, AD0==> GND and INT to pin2 the second gyro: gnd==>gnd, SCL==> A5, SCA ==> A4 and AD0==> GND. int to pin 3.

include

include

include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation // is used in I2Cdev.h

if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include <Wire.h>

endif

// class default I2C address is 0x68 // specific I2C addresses may be passed as a parameter here //AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) //AD0 high = 0x69 MPU6050 mpu=0x68; MPU6050 mpu1= 0x69; // <-- use for AD0 high

/* ========================================================================= NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch depends on the MPU-6050's INT pin being connected to the Arduino's external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is digital I/O pin 2.

• ========================================================================= */

/* ========================================================================= NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error when using Serial.write(buf, len). The Teapot output uses this method. The solution requires a modification to the Arduino USBAPI.h file, which is fortunately simple, but annoying. This will be fixed in the next IDE release. For more info, see these links:

http://arduino.cc/forum/index.php/topic,109987.0.html http://code.google.com/p/arduino/issues/detail?id=958

• ========================================================================= */

// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual // quaternion components in a [w, x, y, z] format (not best for parsing // on a remote host such as Processing or something though) //#define OUTPUT_READABLE_QUATERNION

// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles // (in degrees) calculated from the quaternions coming from the FIFO. // Note that Euler angles suffer from gimbal lock (for more info, see // http://en.wikipedia.org/wiki/Gimbal_lock) //#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ // pitch/roll angles (in degrees) calculated from the quaternions coming // from the FIFO. Note this also requires gravity vector calculations. // Also note that yaw/pitch/roll angles suffer from gimbal lock (for // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)

define OUTPUT_READABLE_YAWPITCHROLL

define OUTPUT_READABLE_YAWPITCHROLL1

// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration // components with gravity removed. This acceleration reference frame is // not compensated for orientation, so +X is always +X according to the // sensor, just without the effects of gravity. If you want acceleration // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. //#define OUTPUT_READABLE_REALACCEL

// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration // components with gravity removed and adjusted for the world frame of // reference (yaw is relative to initial orientation, since no magnetometer // is present in this case). Could be quite handy in some cases. //#define OUTPUT_READABLE_WORLDACCEL

// uncomment "OUTPUT_TEAPOT" if you want output that matches the // format used for the InvenSense teapot demo //#define OUTPUT_TEAPOT

define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards

define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

bool blinkState = false;

// MPU control/status vars bool dmpReady = false; // set true if DMP init was successful uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount; // count of all bytes currently in FIFO uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars Quaternion q; // [w, x, y, z] quaternion container VectorInt16 aa; // [x, y, z] accel sensor measurements VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity; // [x, y, z] gravity vector float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

// MPU control/status vars 1 bool dmp1Ready = false; // set true if DMP init was successful uint8_t mpu1IntStatus; // holds actual interrupt status byte from MPU uint8_t devStatus1; // return status after each device operation (0 = success, !0 = error) uint16_t packetSize1; // expected DMP packet size (default is 42 bytes) uint16_t fifoCount1; // count of all bytes currently in FIFO uint8_t fifoBuffer1[64]; // FIFO storage buffer

// orientation/motion vars Quaternion q1; // [w, x, y, z] quaternion container VectorInt16 aa1; // [x, y, z] accel sensor measurements VectorInt16 aaReal1; // [x, y, z] gravity-free accel sensor measurements VectorInt16 aaWorld1; // [x, y, z] world-frame accel sensor measurements VectorFloat gravity1; // [x, y, z] gravity vector float euler1[3]; // [psi, theta, phi] Euler angle container float ypr1[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo uint8_t teapotPacket1[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

// ================================================================ // === INTERRUPT DETECTION ROUTINE === // ================================================================

volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high volatile bool mpu1Interrupt = false; void dmpDataReady() { mpuInterrupt = true; }

int rawnumber=0; //number in the table printing // ================================================================ // === INTERRUPT DETECTION ROUTINE 1 === // ================================================================

void dmp1DataReady() { mpu1Interrupt = true; }

int rawnumber1=0; //number in the table printing

// ================================================================ // === INITIAL SETUP === // ================================================================

void setup() { // join I2C bus (I2Cdev library doesn't do this automatically)

if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

    Wire.begin();
    Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
#endif

// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately

// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
// Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.

// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
mpu1.initialize();
pinMode(INTERRUPT_PIN, INPUT);
pinMode(3,INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
Serial.println(mpu1.testConnection() ? F("MPU6050(1) connection successful") : F("MPU6050(1) connection failed")); 

// wait for ready
//Serial.println(F("\nSend any character to begin DMP programming and demo: "));

// while (Serial.available() && Serial.read()); // empty buffer //while (!Serial.available()); // wait for data //while (Serial.available() && Serial.read()); // empty buffer again

Serial.println("wait for the values to stabilize after the initialization, it may take a few seconds");
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
devStatus1 = mpu1.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(25);
mpu.setYGyroOffset(-6);
mpu.setZGyroOffset(43);
mpu.setZAccelOffset(1187); // 1688 factory default for my test chip

mpu1.setXGyroOffset(25);
mpu1.setYGyroOffset(-6);
mpu1.setZGyroOffset(43);
mpu1.setZAccelOffset(1187); // 1688 factory default for my test chip

// make sure it worked (returns 0 if so)
if (devStatus == 0) {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP..."));
    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

// make sure it worked (returns 0 if so) if (devStatus1 == 0) { // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP1...")); mpu1.setDMPEnabled(true);

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection1 (Arduino external interrupt 0)..."));
    attachInterrupt(digitalPinToInterrupt(3), dmp1DataReady, RISING);
    mpu1IntStatus = mpu1.getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt1..."));
    dmp1Ready = true;

    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();

    packetSize1 = mpu1.dmpGetFIFOPacketSize();
} 
else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(devStatus);
    Serial.print(devStatus1);
    Serial.println(F(")"));
}

// configure LED for output
pinMode(LED_PIN, OUTPUT);

} }

// ================================================================ // === MAIN PROGRAM LOOP === // ================================================================

void loop() { // if programming failed, don't try to do anything if (!dmpReady) return; if (!dmp1Ready) return; // wait for MPU interrupt or extra packet(s) available

// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();

mpu1Interrupt = false;
mpu1IntStatus = mpu1.getIntStatus();

// get current FIFO count
fifoCount = mpu.getFIFOCount();
fifoCount1 = mpu1.getFIFOCount();

// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    Serial.println(F("FIFO overflow!"));
if ((mpu1IntStatus & 0x10) || fifoCount1 == 1024) {
    // reset so we can continue cleanly
    mpu1.resetFIFO();
    Serial.println(F("FIFO1 overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
} 
 if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);

    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount -= packetSize;

    #ifdef OUTPUT_READABLE_QUATERNION
        // display quaternion values in easy matrix form: w x y z
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        Serial.print("quat\t");
        Serial.print(q.w);
        Serial.print("\t");
        Serial.print(q.x);
        Serial.print("\t");
        Serial.print(q.y);
        Serial.print("\t");
        Serial.println(q.z);
    #endif

    #ifdef OUTPUT_READABLE_EULER
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetEuler(euler, &q);
        Serial.print("euler\t");
        Serial.print(euler[0] * 180/M_PI);
        Serial.print("\t");
        Serial.print(euler[1] * 180/M_PI);
        Serial.print("\t");
        Serial.println(euler[2] * 180/M_PI);
    #endif

    #ifdef OUTPUT_READABLE_YAWPITCHROLL
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

        rawnumber++;
        Serial.print(rawnumber);
        Serial.print('\t');
        Serial.print(ypr[0] * 180/M_PI);
        Serial.print('\t');
        Serial.print(ypr[1] * 180/M_PI);
        Serial.print('\t');
        Serial.print(ypr[2] * 180/M_PI);
        Serial.print('\t');
    #endif

    #ifdef OUTPUT_READABLE_REALACCEL
        // display real acceleration, adjusted to remove gravity
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetAccel(&aa, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
        Serial.print("areal\t");
        Serial.print(aaReal.x);
        Serial.print("\t");
        Serial.print(aaReal.y);
        Serial.print("\t");
        Serial.println(aaReal.z);
    #endif

    #ifdef OUTPUT_READABLE_WORLDACCEL
        // display initial world-frame acceleration, adjusted to remove gravity
        // and rotated based on known orientation from quaternion
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetAccel(&aa, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
        mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
        mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
        Serial.print("aworld\t");
        Serial.print(aaWorld.x);
        Serial.print("\t");
        Serial.print(aaWorld.y);
        Serial.print("\t");
        Serial.println(aaWorld.z);
    #endif

    #ifdef OUTPUT_TEAPOT
        // display quaternion values in InvenSense Teapot demo format:
        teapotPacket[2] = fifoBuffer[0];
        teapotPacket[3] = fifoBuffer[1];
        teapotPacket[4] = fifoBuffer[4];
        teapotPacket[5] = fifoBuffer[5];
        teapotPacket[6] = fifoBuffer[8];
        teapotPacket[7] = fifoBuffer[9];
        teapotPacket[8] = fifoBuffer[12];
        teapotPacket[9] = fifoBuffer[13];
        Serial.write(teapotPacket, 14);
        teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
    #endif

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}
 if (mpu1IntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount1 < packetSize1) fifoCount1 = mpu1.getFIFOCount();

    // read a packet from FIFO
    mpu1.getFIFOBytes(fifoBuffer1, packetSize1);

    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount1 -= packetSize1;

    #ifdef OUTPUT_READABLE_QUATERNION1
        // display quaternion values in easy matrix form: w x y z
        mpu1.dmpGetQuaternion(&q1, fifoBuffer1);
        Serial.print("quat\t");
        Serial.print(q1.w);
        Serial.print("\t");
        Serial.print(q1.x);
        Serial.print("\t");
        Serial.print(q1.y);
        Serial.print("\t");
        Serial.println(q1.z);
    #endif

    #ifdef OUTPUT_READABLE_EULER1
        // display Euler angles in degrees
        mpu1.dmpGetQuaternion(&q1, fifoBuffer1);
        mpu1.dmpGetEuler(euler1, &q1);
        Serial.print("euler\t");
        Serial.print(euler1[0] * 180/M_PI);
        Serial.print("\t");
        Serial.print(euler1[1] * 180/M_PI);
        Serial.print("\t");
        Serial.println(euler1[2] * 180/M_PI);
    #endif

    #ifdef OUTPUT_READABLE_YAWPITCHROLL1
        // display Euler angles in degrees
        mpu1.dmpGetQuaternion(&q1, fifoBuffer1);
        mpu1.dmpGetGravity(&gravity1, &q1);
        mpu1.dmpGetYawPitchRoll(ypr1, &q1, &gravity1);

        rawnumber1++;
        Serial.print(rawnumber1);
        Serial.print('\t');
        Serial.print(ypr1[0] * 180/M_PI);
        Serial.print('\t');
        Serial.print(ypr1[1] * 180/M_PI);
        Serial.print('\t');
        Serial.print(ypr1[2] * 180/M_PI);
         Serial.println('\t');
    #endif

    #ifdef OUTPUT_READABLE_REALACCEL1
        // display real acceleration, adjusted to remove gravity
        mpu1.dmpGetQuaternion(&q1, fifoBuffer1);
        mpu1.dmpGetAccel(&aa1, fifoBuffer1);
        mpu1.dmpGetGravity(&gravity1, &q1);
        mpu1.dmpGetLinearAccel(&aaReal1, &aa1, &gravity1);
        Serial.print("areal1\t");
        Serial.print(aaReal1.x);
        Serial.print("\t");
        Serial.print(aaReal1.y);
        Serial.print("\t");
        Serial.println(aaReal1.z);
    #endif

    #ifdef OUTPUT_READABLE_WORLDACCEL1
        // display initial world-frame acceleration, adjusted to remove gravity
        // and rotated based on known orientation from quaternion
        mpu1.dmpGetQuaternion(&q1, fifoBuffer1);
        mpu1.dmpGetAccel(&aa1, fifoBuffer1);
        mpu1.dmpGetGravity(&gravity1, &q1);
        mpu1.dmpGetLinearAccel(&aaReal1, &aa1, &gravity1);
        mpu1.dmpGetLinearAccelInWorld(&aaWorld1, &aaReal1, &q1);
        Serial.print("aworld1\t");
        Serial.print(aaWorld1.x);
        Serial.print("\t");
        Serial.print(aaWorld1.y);
        Serial.print("\t");
        Serial.println(aaWorld1.z);
    #endif

    #ifdef OUTPUT_TEAPOT1
        // display quaternion values in InvenSense Teapot demo format:
        teapotPacket1[2] = fifoBuffer1[0];
        teapotPacket1[3] = fifoBuffer1[1];
        teapotPacket1[4] = fifoBuffer1[4];
        teapotPacket1[5] = fifoBuffer1[5];
        teapotPacket1[6] = fifoBuffer1[8];
        teapotPacket1[7] = fifoBuffer1[9];
        teapotPacket1[8] = fifoBuffer1[12];
        teapotPacket1[9] = fifoBuffer1[13];
        Serial.write(teapotPacket1, 14);
        teapotPacket1[11]++; // packetCount, loops at 0xFF on purpose
    #endif

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}

} }