stabilisateur de drone

#include "Wire.h"

int gyro_address = 0x68,a=0;
int16_t acc_x,acc_y,acc_z,gyro_x,gyro_y,gyro_z,temp;
double acc_x_bis,acc_y_bis,acc_z_bis,gyro_z_bis,gyro_y_bis,gyro_x_bis;
double acc_x_cal,acc_y_cal,acc_z_cal,gyro_z_cal,gyro_y_cal,gyro_x_cal;
float angle=0,error,setpoint=0,error_variation=0,error_last=0,pid_integral_last=0,pid_derivative=0,pid_proportionnel=0,pid_integral=0,control=0,timer_channel_1;
float timer_channel_2,loop_timer,esc_1,esc_2,esc_loop_timer;
const float kp=2;
const float ki=3;
const float kd=4;
const float throttle=1400;

void MPU_config(){
  // configuration de l'accéléromètre
  Wire.beginTransmission(gyro_address);
  Wire.write(0x1C);   // registre qui contient les bits de configuration AFS_SEL
  Wire.write(0x00);   // AFS_SEL = 0 (10) échelle = 16384
  Wire.endTransmission(true);

  // Configuration du gyromètre
  Wire.beginTransmission(gyro_address);
  Wire.write(0x1B);   // registre qui contient les bits de configuration FS_SEL
  Wire.write(0x00);   // FS_SEL=0 échelle = 131
  Wire.endTransmission(true);
}

void MPU_read_data(){
  Wire.beginTransmission(gyro_address);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(gyro_address,14);
  acc_x = Wire.read()<<8 | Wire.read();
  acc_y = Wire.read()<<8 | Wire.read();
  acc_z = Wire.read()<<8 | Wire.read();
  gyro_x = Wire.read()<<8 | Wire.read();
  gyro_y = Wire.read()<<8 | Wire.read();
  gyro_z = Wire.read()<<8 | Wire.read();
  acc_x_bis = (float)acc_x /16384;
  acc_y_bis = (float)acc_y /16384;
  acc_z_bis = (float)acc_z /16384;
  gyro_x_bis = (float)gyro_x /131;
  gyro_y_bis = (float)gyro_y /131;
  gyro_z_bis = (float)gyro_z /131;
  if(a==2000){
    acc_x_bis-=acc_x_cal;
    acc_y_bis-=acc_y_cal;
    acc_z_bis-=acc_z_cal;
    gyro_x_bis-=gyro_x_cal;
    gyro_y_bis-=gyro_y_cal;
    gyro_z_bis-=gyro_z_cal;
  
    
  }
 
}    
    void MPU_cal(){
        
      for (a = 0; a <= 2000 ; a ++)  {   
                              
       MPU_read_data();
       
       
        gyro_x_cal += gyro_x_bis;                                              
        gyro_y_cal += gyro_y_bis;                                               
        gyro_z_cal += gyro_z_bis;                                               
        acc_x_cal += acc_x_bis;                                              
        acc_y_cal += acc_y_bis;                                               
        acc_z_cal += acc_z_bis-1;  
   }             
                                                                 
      gyro_x_cal /= 2000;                                                        
      gyro_y_cal /= 2000;                                                        
      gyro_z_cal /= 2000;    
      acc_x_cal /= 2000;                                                        
      acc_y_cal /= 2000;                                                        
      acc_z_cal /= 2000;
       

}

void pid_balance(){
  error=setpoint-angle;
  error_variation=error-error_last;
  pid_proportionnel = kp*error;
  pid_integral =pid_integral_last+ki*error ;
  pid_derivative = kd*error_variation;
  control = pid_proportionnel+pid_integral+pid_derivative;
  error_last=error;
  pid_integral_last=pid_integral;
 
 esc_1=throttle;
 esc_2=throttle;
  
  
 // Serial.print("esc_ =  ");
  //Serial.println(esc_1);
  //Serial.print("esc_2 =  ");
  //Serial.println(esc_2);
}
 

 void esc_pulse_output(){
   loop_timer = micros();                                                    //Set the timer for the next loop.

  PORTD |= B01100000 ;                                               //Set digital outputs 4,5.
  timer_channel_1 = esc_1 + loop_timer;                                     //Calculate the time of the faling edge of the esc-1 pulse.
  timer_channel_2 = esc_2 + loop_timer;                                     //Calculate the time of the faling edge of the esc-2 pulse.
 
  while(PORTD >= 16){                                                       //Stay in this loop until output 4,5 are low.
    esc_loop_timer = micros();                                              //Read the current time.
    if(timer_channel_1 <= esc_loop_timer)PORTD &= B01000000;                //Set digital output 4 to low if the time is expired.
    if(timer_channel_2 <= esc_loop_timer)PORTD &= B00100000;  //Set digital output 5 to low if the time is expired.
  }
 }
  
  

void setup() {
  Wire.begin();
  
  Serial.begin(9600);
  Wire.beginTransmission(gyro_address); // Begins a transmission to the I2C slave (GY-521 board)
  Wire.write(0x6B); // PWR_MGMT_1 register
  Wire.write(0); // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  
  MPU_config();
  MPU_read_data();
      
  MPU_cal();
  pid_balance();
}

void loop() {
  MPU_read_data();
    //Serial.print("acc_x =  ");
  //Serial.println(acc_x_bis);
//Serial.print(" | acc_y = ");
// Serial.println(acc_y_bis);
 // Serial.print(" | acc_z = ");
// Serial.println(acc_z_bis);
 
  //Serial.print(" | gyro_x = ");
  //Serial.println(gyro_x_bis);
  //Serial.print(" | gyro_y = ");
  //Serial.println(gyro_y_bis);
  //Serial.print(" | gyro_z = ");
  //Serial.println(gyro_z_bis);
 //  Serial.print(" | esc_1 = ");
  //Serial.println(esc_1);
   //Serial.print(" | esc_2 = ");
  //Serial.println(esc_2);
  
  angle= atan2((double)acc_y,(double)acc_z )*180/PI; // 180/PI permet d'avoir la valeur en °
  Serial.println(angle);
  pid_balance();
  esc_pulse_output();
  
  

 
delay(200);
}