#include <Wire.h>
const int MPU = 0x68; // MPU6050 I2C address
int16_t GyroX ,GyroY,GyroZ,acc_y,acc_x,acc_z;
 float GyroY_avg,angelPitch,acc_y_avg,acc_total_vector ,acc_x_avg , acc_z_avg , angle_pitch_acc;
boolean set_gyro_angles;
float Pcontroller , Icontroller , Dcontroller , output , angelPitchL;
float Kp,Ki,Kd;
int t=0;
void setup() {
  Kp =100;
  Ki = 100;
  Kd = 1.5;
  Pcontroller = 0 ; 
  Dcontroller = 0 ;
  angelPitchL=0;
   Wire.begin();                      // Initialize comunication
  Wire.beginTransmission(MPU);       // Start communication with MPU6050 // MPU=0x68
  Wire.write(0x6B);                  // Talk to the register 6B
  Wire.write(0x00);                  // Make reset - place a 0 into the 6B register
  Wire.endTransmission(true);        //end the transmission
  
  // Configure Accelerometer Sensitivity - Full Scale Range (default +/- 2g)
  Wire.beginTransmission(MPU);
  Wire.write(0x1C);                  //Talk to the ACCEL_CONFIG register (1C hex)
  Wire.write(0x08);                  //Set the register bits as 00010000 (+/- 8g full scale range)
  Wire.endTransmission(true);
  // Configure Gyro Sensitivity - Full Scale Range (default +/- 250deg/s)
  Wire.beginTransmission(MPU);
  Wire.write(0x1B);                   // Talk to the GYRO_CONFIG register (1B hex)
  Wire.write(0x00);                   // Set the register bits as 00010000 (1000deg/s full scale)
  Wire.endTransmission(true);
  pinMode(D5,OUTPUT);
  pinMode(D6,OUTPUT);
  pinMode(D4,OUTPUT);
  digitalWrite(D5,HIGH);
  digitalWrite(D6,HIGH);
  digitalWrite(D4,HIGH);

  delay(20);// put your setup code here, to run once:
Serial.begin(115200);

for(int i=0 ; i<200; i++){
  readdd();
  GyroY += GyroY;

  delay(4);
  }
GyroY_avg = GyroY/2000;
  digitalWrite(D4,LOW);
}

void loop() {
  t++; if (t==10){ 
     digitalWrite(D4,HIGH);
    }
  if(t==20){ digitalWrite(D4,LOW); t = 0;}
readdd();
  GyroY -= GyroY_avg; 

  angelPitch += GyroY*0.0000763;
 

    acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z));  //Calculate the total accelerometer vector
  //57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
  angle_pitch_acc = asin((float)acc_x/acc_total_vector)* 57.296;   

   angelPitch = angelPitch *0.99988 + angle_pitch_acc * 0.00012;  
//  if(set_gyro_angles){                                                 //If the IMU is already started
//    angelPitch = angelPitch * 0.999 + angle_pitch_acc * 0.0010;     //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
//           //Correct the drift of the gyro roll angle with the accelerometer roll angle
//  }
//  else{                                                                //At first start
//    angelPitch = angle_pitch_acc;                                     //Set the gyro pitch angle equal to the accelerometer pitch angle                                     //Set the gyro roll angle equal to the accelerometer roll angle 
//    set_gyro_angles = true;                                            //Set the IMU started flag
//  } 
    //Serial.print(t);
  // Serial.println(""); 
   Serial.println(angelPitch);
    //Serial.println(""); 
     //Serial.println(F(""));
//    Serial.print(F(" "));
//    Serial.print(angle_pitch_acc);
//    Serial.println(F(",")); 

if(angelPitch>0){ 
//P controller --> P = Kp * angelPitch;
Pcontroller = Kp * angelPitch;
Dcontroller = Kd*( angelPitch - angelPitchL)/0.01;
angelPitchL = angelPitch; 
output = Dcontroller + Pcontroller; 
if(output>1022) {  output = 1022;  } 
if(output<0){output= -1 * output;  }
 analogWrite(D5,output); analogWrite(D6,HIGH); 
    Serial.println("output");
    Serial.print(output);

    

}
if(angelPitch<0){ 
//P controller --> P = Kp * angelPitch; 
Pcontroller = Kp * angelPitch;
Dcontroller = Kd*( angelPitch - angelPitchL)/0.01;
angelPitchL = angelPitch; 
output = Dcontroller + Pcontroller; 
if(output>1022) {  output = 1022;  } 
if(output<0){output= -1 * output;  }
 analogWrite(D6,output); analogWrite(D5,HIGH); 
     Serial.println("output");
    Serial.print(output);
}
delay(10);
//for( int i ; i<200; i++){
//  GyroY_avg +=GyroY; 
//  //delay(2);
//  }
  //Serial.println(GyroY_avg/200);
  }

  void readdd(){
    
      Wire.beginTransmission(MPU);
  Wire.write(0x43); // Gyro data first register address 0x43
  Wire.endTransmission(false);
  Wire.requestFrom(MPU, 6, true); // Read 4 registers total, each axis value is stored in 2 registers
  GyroX = (Wire.read() << 8 | Wire.read());  // For a 250deg/s range we have to divide first the raw value by 131.0, according to the datasheet
  GyroY = (Wire.read() << 8 | Wire.read()); 
  GyroZ = (Wire.read() << 8 | Wire.read());
  Wire.beginTransmission(MPU);                                        //Start communicating with the MPU-6050
  Wire.write(0x3B);                                                    //Send the requested starting register
  Wire.endTransmission();                                              //End the transmission
  Wire.requestFrom(MPU, 6, true);                                          //Request 14 bytes from the MPU-6050                                       //Wait until all the bytes are received
  acc_x = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the acc_x variable
  acc_y = Wire.read()<<8|Wire.read();                                  //Add the low and high byte to the acc_y variable
  acc_z = Wire.read()<<8|Wire.read();          
    
    }