#include <Wire.h>
const int MPU1 = 0x68; // Adres pierwszego żyroskopu MPU6050
const int MPU2 = 0x69; // Adres drugiego żyroskopu MPU6050
const int MPU3 = 0x6A; // Adres trzeciego żyroskopu MPU6050
const int MPU4 = 0x6B; // Adres czwartego żyroskopu MPU6050

// Zainicjalizuj zmienne do przechowywania danych o rotacji
int rotation1, rotation2, rotation3, rotation4;

void setup() {
  Wire.begin(); // Inicjalizuj komunikację I2C
  Serial.begin(9600); // Inicjalizuj połączenie z monitorem szeregowym

  Wire.beginTransmission(MPU1);
  Wire.write(0x6B); // Rejestr Power Management
  Wire.write(0); // Wyłącz cyfrowy filtr wstępnego układu wibracji
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU2);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU3);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);

  Wire.beginTransmission(MPU4);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
}

void loop() {
  // Odczytaj dane z żyroskopu 1
  Wire.beginTransmission(MPU1);
  Wire.write(0x43); // Rejestr odczytu rotacji w osi X
  Wire.endTransmission(false);
  Wire.requestFrom(MPU1, 2, true);
  rotation1 = Wire.read() << 8 | Wire.read();

  // Odczytaj dane z żyroskopu 2
  Wire.beginTransmission(MPU2);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU2, 2, true);
  rotation2 = Wire.read() << 8 | Wire.read();

  // Odczytaj dane z żyroskopu 3
  Wire.beginTransmission(MPU3);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU3, 2, true);
  rotation3 = Wire.read() << 8 | Wire.read();

  // Odczytaj dane z żyroskopu 4
  Wire.beginTransmission(MPU4);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU4, 2, true);
  rotation4 = Wire.read() << 8 | Wire.read();

  // Wyświetl dane na serial monitorze
  Serial.print("Rotation 1: ");
  Serial.print(rotation1);
  Serial.print(" degrees\t");

  Serial.print("Rotation 2: ");
  Serial.print(rotation2);
  Serial.print(" degrees\t");

  Serial.print("Rotation 3: ");
  Serial.print(rotation3);
  Serial.print(" degrees\t");

  Serial.print("Rotation 4: ");
  Serial.print(rotation4);
  Serial.println(" degrees");

  delay(500); // Poczekaj 0,5 sekundy
}