   Serial.print(",");
   Serial.print(motorBR);
   Serial.print(",");
-  Serial.print(pid_roll_speed_in);
-  Serial.print(",");
-  Serial.print(pid_pitch_speed_in);
-  Serial.print(",");
-  Serial.println(pid_yaw_speed_in);
+  Serial.print(roll_speed);
+  Serial.print(",");
+  Serial.print(pitch_speed);
+  Serial.print(",");
+  Serial.println(yaw_speed);
 }
 
 void print_pid_out() {

// PID
bool auto_stabilisation_mode = false; // Activer ou pas le mode auto-stabilisé
int dt = 3;
double pid_roll_speed_in, pid_roll_angle_in,   pid_roll_out,   pid_roll_setpoint,  roll_error,  Integral_roll_error,  Derivative_roll_error,  last_roll_error, AS_roll_error, AS_Integral_roll_error, AS_Derivative_roll_error, AS_last_roll_error = 0;
double pid_pitch_speed_in, pid_pitch_angle_in,  pid_pitch_out,  pid_pitch_setpoint, pitch_error, Integral_pitch_error, Derivative_pitch_error, last_pitch_error, AS_pitch_error, AS_Integral_pitch_error, AS_Derivative_pitch_error, AS_last_pitch_error = 0;
double pid_yaw_speed_in,    pid_yaw_out,       pid_yaw_setpoint,   yaw_error,   Integral_yaw_error,   Derivative_yaw_error,   last_yaw_error = 0;

// MOTORS
int motorFR, motorBL, motorFL, motorBR;

// RX
int throttle = THROTTLE_RMIN;
volatile int input[4], arm;
volatile unsigned long chrono_start0, chrono_start1, chrono_start2, chrono_start3, chrono_start4;
volatile int last_interrupt_time0, last_interrupt_time1, last_interrupt_time2, last_interrupt_time3, last_interrupt_time4;


const float pi = 3.14159265359;
Adafruit_BNO055 bno = Adafruit_BNO055();




// Fonction qui s'execute une fois et au début
void setup() {
  Serial.begin(9600);
  //BNO init
  bno.begin();
  bno.setExtCrystalUse(true);
  
  //RX init
  attachInterrupt(A0, calcSignal0, CHANGE);
  attachInterrupt(A1, calcSignal1, CHANGE);
  attachInterrupt(A2, calcSignal2, CHANGE);
  attachInterrupt(A3, calcSignal3, CHANGE);
  attachInterrupt(A4, calcSignal4, CHANGE);

  //Motors init
  esc_1.attach(MOTOR_PIN_FRONT_RIGHT);
  esc_2.attach(MOTOR_PIN_FRONT_LEFT);
  esc_3.attach(MOTOR_PIN_BACK_LEFT);
  esc_4.attach(MOTOR_PIN_BACK_RIGHT);
}


// Fonction qui s'execute a chaque cycle
void loop() {
  
  //IMU calculations
  if (auto_stabilisation_mode == false) {
    imu::Vector<3> gyroscope = bno.getVector(Adafruit_BNO055::VECTOR_GYROSCOPE);
    roll_speed = gyroscope.x() * 180 / pi;
    pitch_speed = gyroscope.y() * 180 / pi;
    yaw_speed = gyroscope.z() * 180 / pi;
  }else{
    imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
    roll_angle = euler.z();
    pitch_angle = euler.y();
    yaw_angle = euler.x();
  }


  //MOTORS
  throttle = map(input[2], THROTTLE_RMIN, THROTTLE_RMAX, MOTOR_ZERO_LEVEL, MOTOR_MAX_LEVEL);
  //PID
  pid_compute();
  // Ecrire les valeurs aux moteurs
  motorFR = throttle + pid_pitch_out + pid_roll_out - pid_yaw_out;
  motorFL = throttle + pid_pitch_out - pid_roll_out + pid_yaw_out;
