#include <Wire.h>

int gyi2cAdres = 0; // 0x68 - 104 dla niskiego lub 0x69 -105 dla stanu wysokiego
float X_out,Y_out,Z_out;
void setup()
{
  Wire.begin();
  Serial.begin(115200);
   I2CAdres();
   //Kalibracja();

}

void loop()
{
Wire.beginTransmission(gyi2cAdres);
Wire.write(59);
Wire.endTransmission(false);
Wire.requestFrom(gyi2cAdres,6,true);
X_out = (Wire.read()|Wire.read()<<8);
Y_out = (Wire.read()|Wire.read()<<8);
Z_out = (Wire.read()|Wire.read()<<8);
X_out = Mapowanie(X_out);
Y_out = Mapowanie(Y_out);
Z_out = Mapowanie(Z_out);

Serial.println((String)X_out+" "+Y_out+" "+Z_out+"\n");
delay(500);


}



void I2CAdres()
{
    byte blad, adres;

  for(adres = 1; adres < 127; adres++ )
  {
    Wire.beginTransmission(adres);
    blad = Wire.endTransmission();
    if (blad == 0) 
    {
      Serial.print("Jest komunikacja z adresem ");
      if (adres<16) 
      {
        Serial.print("0");
      }
    
      gyi2cAdres = adres;
      Serial.println(gyi2cAdres);
     
    }
    else if (blad==4) {
      Serial.print("Jest adres ale bredzi - ");
      if (adres<16) {
        Serial.print("0");
      }
      Serial.println(adres);
    }    
  }
  if (gyi2cAdres == 0) 
  {
    Serial.println("Jesteśmy tu sami\n");
  }

} 

// void Kalibracja
// {
//     // KALIBRACJA
// // X 
// Wire.beginTransmission(gyi2cAddress);
// Wire.wire(); //adres rejestru osi x

// //odczyt - 256 = x 
// //poprawka = -Round(x/4) //w LSB 
// Wire.write(poprawka); //wstawić wartość
// Wire.endTransmission();
// delay(10);


// Wire.beginTransmission(gyi2cAddress);
// Wire.wire(); //adres rejestru osi y
// Wire.write(poprawka); 
// Wire.endTransmission();
// delay(10);

// Wire.beginTransmission(gyi2cAddress);
// Wire.wire(); //adres rejestru osi z
// Wire.write(poprawka); 
// Wire.endTransmission();
// delay(10);
//}

float Mapowanie(float dane)
{
  if(dane>1000)
  {
    dane=dane/256;
  }
  return dane;
}