Electronic – arduino – Data Problem with Multiple MPU6050 and Arduino Nano

I've been working on a recent project which involves multiple IMU's(Specifically 5 MPU6050's, GY-521 Breakout board). Everything works fine, from angle calculation to dealing with gyroscopic drift for TWO sensors. The problem comes when I add in a third sensor.

Basically what happens is, when printing out the X/Y values(on the Serial Monitor) of two sensors(S1 and S2) they work out fine, but when a third sensor(S3) is included, the angle values from S1 completely mirror for the S3 values. I performed multiple tests(Shorts, Software/Hardware, Swapping/Switching sensors, Swapping Standby and Reading address, etc.) yet the results are the same.

When I run the function called "ThirdGyro" even without printing it, is when things go haywire. I'm using the "CD4051" Demux to switch between the sensors.

The MPU6050 has 2 addresses in which it can be read on, 0x68 – Default, and 0x69. The way I do it for 3 sensors, is by reading the values from the sensor which has its AD0 pin set HIGH,(0x69) which I've set as the reading address (0x68 as standby/not read). With the common output(3) set high on the CD4051, and all ADO pins of the IMU connected to their respective I/O's, by Demuxing I can isolate each sensor and have it read.

I didn't run the SCL and SDA pins of the IMU, through the Demux just cause I thought it was a hassle. Also guys this is my first question 🙂

//Necessary Libraries
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyroSB(0x68); // SB - Standby Gyro, Not Read
MPU6050 accelgyroR(0x69); // R - Reading gyro, AD0 = H then being Read

//Global Gyro/Accel Variables for RAW DATA
int16_t gx, gy, gz, gsx, gsy, gsz, ax, ay, az, asx, asy, asz;

double timeStep, time, timePrev;
double arx, ary, arz, grx, gry, grz, rx, ry, rz;
int i;

//Filtered Gyro/Accel Variables for Sensors: r[Axis] [Sensor#]
int rx1, rx2, rx3, ry1, ry2, ry3, rz1, rz2, rz3;


void setup(){
  Wire.begin(); //Join 12c bus with lib
  Serial.begin(9600);

  accelgyroR.initialize();
  accelgyroSB.initialize();

  //Initialization of Gyroscope
  time = millis();

  i = 1;

  pinMode(3, OUTPUT); //Select Channel Input A
  pinMode(4, OUTPUT); //Select Channel Input B
  pinMode(5, OUTPUT); //Select Channel Input C

  pinMode(6, OUTPUT); //Common Output for HIGH 
  digitalWrite(6, HIGH);

  //Connection Check
  Serial.println("Preliminary Connection Check..");
  Serial.println(accelgyroR.testConnection() ? "MPU6050 #1 connection 
  successful" : "Connection Failed");
  }


  void AngleCalc(){ //Universal Angle Calculation with Complimentary Filter

  timePrev = time;
  time = millis();
  timeStep = (time - timePrev) / 1000; //Step in seconds

  //Gathering Raw Gyroscope Data of Selected Gyro
  accelgyroR.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  //Scaling Raw data with 131(Gyro) and 2048(Accelerometer) scale factors  
  from Datasheet, s=scaled

  gsx = gx / 131;
  gsy = gy / 131;
  gsz = gz / 131;
  asx = ax / 2048;
  asy = ay / 2048;
  asz = az / 2048;

  //Accelerometer angle calculation
  arx = (180/3.141592) * atan(asx / sqrt(square(asy) + square(asz)));
  ary = (180/3.141592) * atan(asy / sqrt(square(asx) + square(asz)));
  arz = (180/3.141592) * atan(sqrt(square(asy) + square(asx)) / asz);

  if (i == 1){
    grx = arx;
    gry = ary;
    grz = arz;
    }
  else{
    grx = grx + (timeStep * gsx);
    gry = gry + (timeStep * gsy);
    grz = grz + (timeStep * gsz);
    }
  //Cocktail of Gyro and Accelerometer data ratioed ;)
  rx = (0.96 * arx) + (0.04 * grx);
  ry = (0.96 * ary) + (0.04 * gry);
  rz = (0.96 * arz) + (0.04 * grz);

  }

void FirstGyro(){
  digitalWrite(3, LOW); //A           ADDRESS : 000
  digitalWrite(4, LOW);  //B
  digitalWrite(5, LOW);  //C
  AngleCalc();
  rx1 = rx;
  ry1 = ry; 
  rz1 = rz;
  delay(20);
}

void SecondGyro(){
  digitalWrite(3, HIGH); //A          ADDRESS : 001
  digitalWrite(4, LOW);  //B
  digitalWrite(5, LOW);  //C
  AngleCalc();
  rx2 = rx;
  ry2 = ry; 
  rz2 = rz;
  delay(20);
}

void ThirdGyro(){
  digitalWrite(3, LOW);  //A          ADDRESS : 010
  digitalWrite(4, HIGH); //B
  digitalWrite(5, LOW);  //C 
  AngleCalc();
  rx3 = rx;
  ry3 = ry;
  rz3 = rz;
  delay(20);
  }

void loop(){

//MUXING THE AD0 PIN SERVES AS IDENTIFYING UNIQUE SENSORS
//BELOW CODE REFERS TO ABOVE RESPECTIVE FUNCTIONS OF RESPECTIVE SENSORS

  FirstGyro();
  delay(10);
  SecondGyro();
  delay(10);
  ThirdGyro();

  Serial.println("");
  Serial.print(ry1); Serial.print("\t");
  Serial.print(ry2); Serial.print("\t");
  Serial.print(ry3); 
  //Print out filtered data

  }  

^{simulate this circuit – Schematic created using CircuitLab}