#include<Wire.h>

 const int MPU_addr=0x68;
 uint32_t timer;
 double AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
 double compRoll, compPitch;


//radians to degree
#define deg 57.2957795

int MPU6050_write_reg(int reg, uint8_t data)
{
  int e;
 e = MPU6050_write(reg, &data, 1);

  return (e);
}

int MPU6050_write(int start, const uint8_t *pData, int size)
{
  int n, k;
  Wire.beginTransmission(0x68);
  n = Wire.write(start);        // write the start address
  if (n != 1)
    return (-20);

  n = Wire.write(pData, size);  // write data bytes
  if (n != size)
    return (-21);

  k = Wire.endTransmission(true); // release the I2C-bus
  if (k != 0)
    return (k);

  return (0);         // return : no error
}



void setup()
{

  // Set I2C frequency to 400kHz
  Wire.begin();
  #if ARDUINO >= 157
  Wire.setClock(400000UL);
  #else
    TWBR = ((F_CPU / 400000UL) - 16) / 2;
  #endif

  //Initial readings from MPU + DLPF activation
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.begin(115200);
  delay(3000);

  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);
  AcX=Wire.read()<<8|Wire.read();
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();
  Tmp=Wire.read()<<8|Wire.read();
  GyX=Wire.read()<<8|Wire.read();
  GyY=Wire.read()<<8|Wire.read();
  GyZ=Wire.read()<<8|Wire.read();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(115200);
  Wire.beginTransmission(MPU_addr);

  MPU6050_write_reg(0x1A, 0x06);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();
  AcY=Wire.read()<<8|Wire.read();
  AcZ=Wire.read()<<8|Wire.read();
  Tmp=Wire.read()<<8|Wire.read();
  GyX=Wire.read()<<8|Wire.read();
  GyY=Wire.read()<<8|Wire.read();
  GyZ=Wire.read()<<8|Wire.read();

  double roll = atan2(AcY, AcZ)*deg;
  double pitch = atan2(-AcX, AcZ)*deg;
  double compRoll = roll;
  double compPitch = pitch;

  timer = micros();
 }

void loop() {

    Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)

  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr,14,true);  // request a total of 14 registers
  AcX=Wire.read()<<8|Wire.read();  // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY=Wire.read()<<8|Wire.read();  // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ=Wire.read()<<8|Wire.read();  // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp=Wire.read()<<8|Wire.read();  // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX=Wire.read()<<8|Wire.read();  // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY=Wire.read()<<8|Wire.read();  // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ=Wire.read()<<8|Wire.read();  // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

  double dt1 = (double)(micros() - timer) / 1000000;
  timer = micros();

  double roll = atan2(AcY, AcZ)*deg;
  double pitch = atan2(-AcX, AcZ)*deg;

  //1 deg=131 raw value
  double gyroXrate = GyX/131.0;
  double gyroYrate = GyY/131.0;
  double gyroZrate= GyZ/131.0;

  //Complementary filter
  compRoll = 0.99 * (compRoll + gyroXrate * dt1) + 0.01 * roll;
  compPitch = 0.99 * (compPitch + gyroYrate * dt1) + 0.01 * pitch;
  double   InputP=compRoll-8.52;
  double   InputR=compPitch-5.83;

 }