#include "Wire.h"
#include <MPU6050_light.h>

MPU6050 mpu(Wire);

unsigned long timer=0;

float gx,gy,gz;
float pitch=0;
float roll=0;
float yaw=0 ;



float timestep;
float mintimestep=40;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.begin();

  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  mpu.calcOffsets(); // gyro and accelero
  Serial.println("Done!\n");
}

void loop() {
  mpu.update();
  timestep = millis()-timer;
  if (timestep>=mintimestep) {
    timer+=timestep;
    gx=mpu.getGyroX();
    gy=mpu.getGyroY();
    gz=mpu.getGyroZ();

   

    
    pitch+=gy*timestep/1000;
    roll+=gx*timestep/1000;
    yaw+=gz*timestep/1000;

    

    
    Serial.print("pitch= ");
    Serial.print(pitch);
    Serial.print(" deg ");	
    Serial.print("roll= ");	
    Serial.print(roll);
    Serial.print(" deg ");	
    Serial.print("yaw= ");	
    Serial.print(yaw);
    Serial.print(" deg ");

    Serial.println();

    Serial.print("acc pitch= ");
    Serial.print(mpu.getAngleX());
    Serial.print(" deg ");	
    Serial.print("acc roll= ");	
    Serial.print(mpu.getAngleY());
    Serial.print(" deg ");	
    Serial.print("acc yaw= ");	
    Serial.print(mpu.getAngleZ());
    Serial.print(" deg ");

    Serial.println();
  }
}