#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <MPU6050_light.h>

#define SCREEN_WIDTH 128 // szerokosc wyswietlacza w pikselach
#define SCREEN_HEIGHT 64 // wysokosc wyswietlacza w pikselach
// ustawienie wyswietlacza i na jakich pinach sie znajduje
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);


MPU6050 mpu(Wire); // znajdowanie zyroskopu na i2c
MPU6050 mpu2(Wire);
unsigned long timer = 0;// do czasu to pominac

void setup() {
  Serial.begin(115200);                           
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))  // adres wyswietlacza
  { 
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);                                     
  } 
  mpu2.setAddress(0x69);  // adres drugiego zyroskopu
  display.setTextSize(1);             
  display.setTextColor(SSD1306_WHITE);            // biala czcionka
  display.clearDisplay();                         
  Wire.begin();
  mpu.begin();
  mpu2.begin();
  display.println(F("Calculating gyro offset, do not move MPU6050"));
  display.display();        
  mpu.calcGyroOffsets();                          // kalibrowanie zyro
  mpu2.calcGyroOffsets();  
  display.setTextSize(1);    
  
}

void loop() {
  mpu.update(); // pobieranie pozycji zyroskopu
  mpu2.update(); 
  
x_1 = mpu.getAngleX();
y_1 = mpu.getAngleY();
z_1 = mpu.getAngleZ();

x_2 = mpu2.getAngleX();
y_2 = mpu2.getAngleY();
z_2 = mpu2.getAngleZ();

  int deg; 
  //deg = mpu.getAngleX() + mpu2.getAngleX(); // obliczanie katu, dodawanie bo zyrokopy sa zwrocone do siebie przeciwnie
  
  if(y_1 + y_2 < 180) 
  {
	  deg = x_1 + x_2;
  } 
  else if( (x_1 + x_2) > 180 && (x_1 + x_2) < 360)
  {
	  deg = y_1 + y_2;
  }
  
  
  if(deg < 0)
  {
    deg = deg+360; // jesli obrocimy zyroskop o wiecej niz 180 bo inacej warosci bylyby ujemne
  }
  if((millis()-timer)>10)                         // odswierzanie wyswietlacza co 10ms
  {                                         
    display.clearDisplay();                       
    display.setCursor(0,0);
    display.print(deg);
    display.println("st.");
    display.print("x:");
    display.println(mpu.getAngleX());
    display.print("y:");
    display.println(mpu.getAngleY());
    display.print("z:");
    display.println(mpu.getAngleZ());              
    display.print("P2:");
    display.println(mpu2.getAngleX());
    display.print("R2:");
    display.println(mpu2.getAngleY());
    display.print("Y2:");
    display.print(mpu2.getAngleZ());
    display.display();
    timer = millis();  
  }
}