// ESP32 with many MPU-6050 sensors
// --------------------------------
// Select a MPU-6050 by changing its I2C address runtime.
//
// 10 April 2024, Koepel, Public Domain.
//
// This Wokwi project: https://wokwi.com/projects/394777263927329793
//
// A MPU-6050 sensor can have its I2C address at 0x68 or 0x69.
// In this project, only the active sensor is set at 0x68.
// All the other sensors are at 0x69.
//
// Warning 1: 
//   In real life, this is only possible with a 3.3V board.
//   A output pin with 5V output may not be connected 
//   directly to pin AD0 of the sensor.
//
// Warning 2:
//   In real life, there might be too many pullup for
//   the I2C bus, because each module has pullup resistors.
//

#include <Wire.h>

// The number of MPU-6050 sensors in this example is 5.
#define NUM_MPUS 5
const int AD0pin[NUM_MPUS] = {16, 17, 18, 19, 23};

const int mpuAddr = 0x68;
const int mpuAddr_not_used = 0x69; // not even used in the code.

void setup() {
  Serial.begin(115200);
  Serial.println("Click on a MPU-6050 module and change the acceleration.");

  for(int i=0; i<NUM_MPUS; i++)
  {
    pinMode(AD0pin[i],OUTPUT);
    digitalWrite(AD0pin[i],HIGH);  // default high for 0x69
  }

  Wire.begin();  // default pins, SDA=21, SCL=22.

  for(int i=0; i<NUM_MPUS; i++)
  {
    SelectMPU(i);
    Wire.beginTransmission(mpuAddr);
    Wire.write(0x6B);  // power register
    Wire.write(0);     // turn sensor on
    Wire.endTransmission();
  }
}

void loop() 
{
  // Get the acceleration
  // With the default settings, the 2-byte signed integer
  // from the sensor corresponds with a g-force range
  // of -2 g to +2 g.
  // The integer is therefor divided by 16384.
  for(int i=0; i<NUM_MPUS; i++)
  {
    SelectMPU(i);
    Wire.beginTransmission(mpuAddr);
    Wire.write(0x3B);  // first register address for acceleration
    Wire.endTransmission(false);
    Wire.requestFrom(mpuAddr, 6);  // x,y,z, two bytes each.
    int16_t ax = Wire.read()<<8 | Wire.read();
    int16_t ay = Wire.read()<<8 | Wire.read();
    int16_t az = Wire.read()<<8 | Wire.read();
    float gx = (float) ax / 16384.0;
    float gy = (float) ay / 16384.0;
    float gz = (float) az / 16384.0;
    Serial.printf("MPU %d = %5.2f,%5.2f,%5.2f\r\n", i, gx, gy, gz);
  }
  Serial.println("-------------------------");

  delay(1500);
}

// Choose a MPU, parameter starts at zero.
// AD0 = high, I2C address = 0x69, not selected
// AD0 = low,  I2C address = 0x68, selected
void SelectMPU(int selection)
{
  for(int i=0; i<NUM_MPUS; i++)
  {
    if(i == selection)
      digitalWrite(AD0pin[i],LOW);  // selected, 0x68
    else
      digitalWrite(AD0pin[i],HIGH); // not selected, 0x69
  }
}