 
 void rx_read()
 {
-//  input[0] = pulseIn(A0, HIGH);//
-//  input[1] = pulseIn(A1, HIGH);//THIS is now interrupts for ARM
-//  input[2] = pulseIn(A2, HIGH);//
-//  input[3] = pulseIn(A3, HIGH);//
+  //TODO: Try with interrupts
+  input[0] = pulseIn(A0, HIGH);
+  input[1] = pulseIn(A1, HIGH);
+  input[2] = pulseIn(A2, HIGH);
+  input[3] = pulseIn(A3, HIGH);
 
 }
 
 void print_rx_values() {
-  Serial.print(input[0]);//Ici on print les valeurs en microsecondes
+  Serial.print(input[0]);//Print values in millis
   Serial.print(" - ");
   Serial.print(input[1]);
   Serial.print(" - ");

  motor_left.writeMicroseconds(mL - 100);
  motor_front.writeMicroseconds(mF - 100);
  motor_back.writeMicroseconds(mB - 100);
  print_pitch_and_roll();

}


#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>

Servo motor_right;
Servo motor_left;
Servo motor_front;
Servo motor_back;

// PID variables
double pid_roll_in,   pid_roll_out,   pid_roll_setpoint = 0;
