/*

Forum: https://forum.arduino.cc/t/2-mpu6050-any-code-is-notworking-at-all/1211476/16
Wokwi: https://wokwi.com/projects/387368306622724097


Special thanks to Uri Shaked for providing the MPU6050 example which I used as a basis
to demo the use of two MPU6050 with one controller

2024/01/19 
ec2021

Source: https://mschoeffler.com/2017/10/05/tutorial-how-to-use-the-gy-521-module-mpu-6050-breakout-board-with-the-arduino-uno/
AD0 (If this pin is LOW, the I2C address of the board will be 0x68. 
Otherwise, if the pin is HIGH, the address will be 0x69.)

*/

// Start the simulation, click on the MPU6050 part, and
// change the x/y/z values to see changes in the plotter.

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#define b1 4
#define b2 5

Adafruit_MPU6050 mpu1;
Adafruit_MPU6050 mpu2;

void setup(void) {
  Serial.begin(115200);
  pinMode(b1, OUTPUT);
  pinMode(b2, OUTPUT);
  // Try to initialize!
  if (!mpu1.begin(0x68)) {
    Serial.println("Failed to find MPU6050 chip on default address");
    while (1) {
      delay(10);
    }
  }

  if (!mpu2.begin(0x69)) {
    Serial.println("Failed to find MPU6050 chip on default address");
    while (1) {
      delay(10);
    }
  }
  prepareMPU(mpu1);
  prepareMPU(mpu2);
  Serial.println("");
  delay(100);
}

unsigned long lastprint = 0;
unsigned long current = millis();

void loop() {
    printMPUData(mpu1, b1);
    printMPUData(mpu2, b2);
}

void prepareMPU(Adafruit_MPU6050 &mpu) {
  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
}

void printMPUData(Adafruit_MPU6050 &mpu,int bpin) {
  /* Get new sensor events with the readings */
  float note[] = {1046.50,1108.73,1174.66,1244.51,1318.51,1396.91,1479.98 ,1567.98,1661.22,1760.00,1864.66,1975.53};
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp); 
  int standis = 15;
  int start = 0;
  float distance = sqrt(pow(a.acceleration.x,2)+pow(a.acceleration.y,2)+pow(a.acceleration.z,2));
  float soundvalue = distance * 3000;   
  /* Print out the values */
  Serial.print("Acceleration(x): ");
  Serial.print(a.acceleration.x);
  Serial.print(" Acceleration(y): ");
  Serial.print(a.acceleration.y);
  Serial.print(" Acceleration(z): ");
  Serial.println(a.acceleration.z);
  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println("m");
  Serial.print("");
  if(distance>standis){
      tone(bpin, soundvalue); 
  }
  else{
    noTone(bpin);
  }
}
/*
void buzz(int bpin){
  float note[] = {1046.50,1108.73,1174.66,1244.51,1318.51,1396.91,1479.98 ,1567.98,1661.22,1760.00,1864.66,1975.53};
  static unsigned long ms_from_start = millis();
  static unsigned long ms_previous_read_buzz
  if(ms_from_start-ms_previous_read_buzz>1000){
    ms_previous_read_buzz = ms_from_start;
    int freq = ms_previous_read_buzz1/2000;
    tone(bpin, note[freq]);
  }
}
*/