   rx_read();
   bno_get_values();
   control_update();
-  print_yaw_and_motors();
-
+  print_motors();
 }



// PID variables
bool auto_stabilisation_mode = true; //activer ou pas le mode auto-stabilisé: utiliser le switch de input 5 ou 6
int dt = 3;//notre dt est de 100 millisecondes
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;

  yaw_speed = gyroscope.z() * 180 / 3.14159265359;

  imu::Vector<3> euler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
  roll_angle = euler.z();
  pitch_angle = euler.y();

  pid_roll_speed_in = map(roll_speed, -180, 180, ROLL_WMIN, ROLL_WMAX);
