Go Down

Topic: Calculating the angle of two MPU6050 simultaneously  (Read 943 times) previous topic - next topic

treehugger

Hello!

I am looking for a way to measure angles with two MPU6050 on the same Adruino simultaneously. I attached the sketch that I am using.

MPU#1 works fine and I get really nice data. However, I get also a data output from MPU#2. The values are all around 134 and do not change when moving the MPU. Did I do anything wring with the calculation or with the wiring? ADO (0x69??) of MPU#2 is not connected to GND, but even connecting it with GND does not help.

I hope someone can help!



treehugger

////////////////////////////////////////////////////////////////
// WIRING OF THE HARDWARE
//
// Arduino - MPU1 - MPU2
// A4 - SDA1 - SDA2
// A5 - SCL1 - SCL2
// 3.3 - VCC1 - VCC2- ADO2
// GND - GND1 - GND2
//
///////////////////////////////////////////////////////////////




#include<Wire.h>
const int MPU_addr=0x68;
const int MPU_addr1=0x69;
double AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
double AcX1,AcY1,AcZ1,Tmp1,GyX1,GyY1,GyZ1;
uint32_t timer;
double compAngleX, compAngleY;
double compAngleX1, compAngleY1;
#define degconvert 57.2957786



void setup() {
  // Set up MPU 6050:
  Wire.begin();
  #if ARDUINO >= 157
  Wire.setClock(400000UL); // Set I2C frequency to 400kHz
  #else
    TWBR = ((F_CPU / 400000UL) - 16) / 2; // Set I2C frequency to 400kHz
  #endif

 
  Wire.beginTransmission(MPU_addr);
  Wire.beginTransmission(MPU_addr1);
  Wire.write(0x6B); 
  Wire.write(0);     
  Wire.endTransmission(true);
  Serial.begin(9600);
  Serial.println("CLEARDATA");
  Serial.println("LABEL, time, compAngleX, compAngleY, compAngleX1, compAngleY1");
  Serial.println("Initializing I2C devices...");
  delay(100);

  //setup starting angle
  //1) collect the data
  Wire.beginTransmission(MPU_addr);
  Wire.beginTransmission(MPU_addr1);
  Wire.write(0x3B); 
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true); 
  Wire.requestFrom(MPU_addr1,14,true); 
  AcX=Wire.read()<<8|Wire.read();     
  AcY=Wire.read()<<8|Wire.read(); 
  AcZ=Wire.read()<<8|Wire.read(); 
  Tmp=Wire.read()<<8|Wire.read();
  GyX=Wire.read()<<8|Wire.read(); 
  GyY=Wire.read()<<8|Wire.read(); 
  GyZ=Wire.read()<<8|Wire.read(); 
  AcX1=Wire.read()<<8|Wire.read();     
  AcY1=Wire.read()<<8|Wire.read(); 
  AcZ1=Wire.read()<<8|Wire.read(); 
  Tmp1=Wire.read()<<8|Wire.read(); 
  GyX1=Wire.read()<<8|Wire.read(); 
  GyY1=Wire.read()<<8|Wire.read(); 
  GyZ1=Wire.read()<<8|Wire.read(); 
 

  double roll = atan2(AcY, AcZ)*degconvert;
  double pitch = atan2(-AcX, AcZ)*degconvert;
  double roll1 = atan2(AcY1, AcZ1)*degconvert;
  double pitch1 = atan2(-AcX1, AcZ1)*degconvert;
 
 
  double gyroXangle = roll;
  double gyroYangle = pitch;
  double compAngleX = roll;
  double compAngleY = pitch;
  double gyroXangle1 = roll1;
  double gyroYangle1 = pitch1;
  double compAngleX1 = roll1;
  double compAngleY1 = pitch1;

  //start a timer
  timer = micros();
}

void loop() {
  Wire.beginTransmission(MPU_addr);
  Wire.beginTransmission(MPU_addr1);
  Wire.write(0x3B); 
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);
  Wire.requestFrom(MPU_addr1,14,true);
  AcX=Wire.read()<<8|Wire.read();       
  AcY=Wire.read()<<8|Wire.read(); 
  AcZ=Wire.read()<<8|Wire.read(); 
  Tmp=Wire.read()<<8|Wire.read(); 
  GyX=Wire.read()<<8|Wire.read(); 
  GyY=Wire.read()<<8|Wire.read(); 
  GyZ=Wire.read()<<8|Wire.read(); 
  AcX1=Wire.read()<<8|Wire.read();     
  AcY1=Wire.read()<<8|Wire.read(); 
  AcZ1=Wire.read()<<8|Wire.read(); 
  Tmp1=Wire.read()<<8|Wire.read(); 
  GyX1=Wire.read()<<8|Wire.read(); 
  GyY1=Wire.read()<<8|Wire.read();
  GyZ1=Wire.read()<<8|Wire.read();
 
  double dt = (double)(micros() - timer) / 1000000;
  double dt1 = (double)(micros() - timer) / 1000000;
  timer = micros();
 
  double roll = atan2(AcY, AcZ)*degconvert;
  double pitch = atan2(-AcX, AcZ)*degconvert;
  double roll1 = atan2(AcY1, AcZ1)*degconvert;
  double pitch1 = atan2(-AcX1, AcZ1)*degconvert;
 
  double gyroXrate = GyX/131.0;
  double gyroYrate = GyY/131.0;
  double gyroXrate1 = GyX1/131.0;
  double gyroYrate1 = GyY1/131.0;
 
  compAngleX = 0.99 * (compAngleX + gyroXrate * dt) + 0.01 * roll;
  compAngleY = 0.99 * (compAngleY + gyroYrate * dt) + 0.01 * pitch;
  compAngleX1 = 0.99 * (compAngleX1 + gyroXrate1 * dt1) + 0.01 * roll1;
  compAngleY1 = 0.99 * (compAngleY1 + gyroYrate1 * dt1) + 0.01 * pitch1;
 
  Serial.print("DATA, TIME,"); Serial.print (compAngleX, 4); 
 Serial.print(","); Serial.print (compAngleY, 4);
 Serial.print(","); Serial.print (compAngleX1, 4); 
 Serial.print(","); Serial.println (compAngleY1, 4);
 }

jremington

#2
Aug 10, 2017, 07:53 pm Last Edit: Aug 10, 2017, 07:54 pm by jremington
When using the I2C bus,  you can read from only one device at a time: begin transmission, read and end transmission. Each device MUST have a unique address.

Sorry, but you should throw that code away and start over.

treehugger

When using the I2C bus,  you can read from only one device at a time: begin transmission, read and end transmission. Each device MUST have a unique address.
Could I do this with a delay-function between readings?

jremington

#4
Aug 10, 2017, 10:34 pm Last Edit: Aug 11, 2017, 12:31 am by jremington
There is no reason to use delay. Read everything of interest from one device, then from the other.

Power_Broker

How did you change the I2C address of the second MPU? Isn't the MPU I2C address read only?
"The desire that guides me in all I do is the desire to harness the forces of nature to the service of mankind."
   - Nikola Tesla

jremington

The AD0 pin sets the low bit of the device address.

treehugger

The AD0 pin sets the low bit of the device address.
I assume this is done by setting the MPU on 0x69:

#include<Wire.h>
const int MPU_addr=0x68;
const int MPU_addr1=0x69;


No clue if it works!

treehugger

When using the I2C bus,  you can read from only one device at a time: begin transmission, read and end transmission. Each device MUST have a unique address.

Sorry, but you should throw that code away and start over.
Done! Separated the transmission of two devices as you said. Works well now. Thanks a lot for the help.

jremington

Quote
I assume this is done by setting the MPU on 0x69:
You also have to connect AD0 on the second MPU to Vcc (not ground or floating).

shaikhhammadali

Done! Separated the transmission of two devices as you said. Works well now. Thanks a lot for the help.
Can i have the rectified code please?

Go Up