/* Kod robota usługowego, wersja tylko dla sensorów

*/
//Biblioteki interfejsu sensora MPU 6050

#include <Wire.h>

//#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro(0x68);

void setup()
{
  
  //uruchomienie portu szeregowego z prędkością transmisji 115200
  Serial.begin(115200);  
  Setup_MPU6050();
}

void Setup_MPU6050()
{
   Wire.begin();
  
     // zainicjowanie urządzenia
    Serial.println("Inicjalizacja urządzeń I2C...");
    accelgyro.initialize();

     // weryfikacja połączenia
    Serial.println("Testowanie połączeń z urządzeniami...");
    Serial.println(accelgyro.testConnection() ? "MPU6050: połączenie udane" : "MPU6050: połączenie nie powiodło się");

 
}

void loop()
{

   //Aktualizacja sensora MPU 6050
    Update_MPU6050();
 
}

void Update_MPU6050()
{
  
   int16_t ax, ay, az;
  int16_t gx, gy, gz;

      // Odczyt surowych danych żyroskopu/akcelerometra z urządzenia
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

    // Wyświetlenie wartości x/y/z akcelerometra/żyroskopu oddzielonych tabulatorem
    Serial.print("i");Serial.print("\t");
    Serial.print(ax); Serial.print("\t");
    Serial.print(ay); Serial.print("\t");
    Serial.print(az); Serial.print("\t");
    Serial.print(gx); Serial.print("\t");
    Serial.print(gy); Serial.print("\t");
    Serial.println(gz);
    Serial.print("\n");

}


