   motorBR = throttle - pid_pitch_out + pid_roll_out + pid_yaw_out;
 
 
-  esc_1.writeMicroseconds(motorFR);
-  esc_2.writeMicroseconds(motorFL);
-  esc_3.writeMicroseconds(motorBL);
-  esc_4.writeMicroseconds(motorBR);
-
-}
+  ///ARM switch
+  if(input[4] > 1500){
+    esc_1.writeMicroseconds(motorFR);
+    esc_2.writeMicroseconds(motorFL);
+    esc_3.writeMicroseconds(motorBL);
+    esc_4.writeMicroseconds(motorBR);
+  }else if (input[4] < 1500) {
+    esc_1.writeMicroseconds(1000);
+    esc_3.writeMicroseconds(1000);
+    esc_2.writeMicroseconds(1000);
+    esc_4.writeMicroseconds(1000);
+  }
+
+  
+  print_motors();
+}
+
+
+////////////////////////////////////////////////////////////////////////
+//INTERRUPTS
+////////////////////////////////////////////////////////////////////////
+void calcSignal0() {
+  last_interrupt_time0 = micros(); 
+  if(digitalRead(A0) == HIGH){ 
+    chrono_start0 = micros();
+  }
+  else{ 
+    if(chrono_start0 != 0){ 
+      input[0] = ((volatile int)micros() - chrono_start0);
+      chrono_start0 = 0;
+    }
+  }
+}
+
+void calcSignal1() {
+  last_interrupt_time1 = micros(); 
+  if(digitalRead(A1) == HIGH){ 
+    chrono_start1 = micros();
+  }
+  else{ 
+    if(chrono_start1 != 0){ 
+      input[1] = ((volatile int)micros() - chrono_start1);
+      chrono_start1 = 0;
+    }
+  }
+}
+
+void calcSignal2() {
+  last_interrupt_time2 = micros(); 
+  if(digitalRead(A2) == HIGH){ 
+    chrono_start2 = micros();
+  }
+  else{ 
+    if(chrono_start2 != 0){ 
+      input[2] = ((volatile int)micros() - chrono_start2);
+      chrono_start2 = 0;
+    }
+  }
+}
+
+void calcSignal3() {
+  last_interrupt_time3 = micros(); 
+  if(digitalRead(A3) == HIGH){ 
+    chrono_start3 = micros();
+  }
+  else{ 
+    if(chrono_start3 != 0){ 
+      input[3] = ((volatile int)micros() - chrono_start3);
+      chrono_start3 = 0;
+    }
+  }
+}
+
+void calcSignal4() {
+  last_interrupt_time4 = micros(); 
+  if(digitalRead(A4) == HIGH){ 
+    chrono_start4 = micros();
+  }
+  else{ 
+    if(chrono_start4 != 0){ 
+      input[4] = ((volatile int)micros() - chrono_start4);
+      chrono_start4 = 0;
+    }
+  }
+}
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//PID controller
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+void pid_compute() {
+  
+  //REDO
+  //ROLL calculations
+  pid_roll_setpoint = 0;
+  if(input[0] > THROTTLE_RMID+10)pid_roll_setpoint = (input[0]-THROTTLE_RMID+10)/2.0;
+  else if(input[0] < THROTTLE_RMID-10)pid_roll_setpoint = (input[0]-THROTTLE_RMID-10)/2.0;
+  
+  roll_error = roll_speed - pid_roll_setpoint;
+  
+  Proportional_roll = ROLL_PID_KP * roll_error;
+  Integral_roll += ROLL_PID_KI * roll_error;
+  if(Integral_roll > ROLL_PID_MAX)Integral_roll = ROLL_PID_MAX;
+  else if(Integral_roll < ROLL_PID_MIN)Integral_roll = ROLL_PID_MIN;
+  Derivative_roll = ROLL_PID_KD * (roll_error - last_roll_error);
+
+  pid_roll_out = Proportional_roll + Integral_roll + Derivative_roll;
+  if(pid_roll_out > ROLL_PID_MAX)pid_roll_out = ROLL_PID_MAX;
+  else if(pid_roll_out < ROLL_PID_MIN)pid_roll_out = ROLL_PID_MIN;
+
+  last_roll_error = roll_error;
+
+
+  //PITCH calculations
+  pid_pitch_setpoint = 0;
+  if(input[1] > THROTTLE_RMID+10)pid_pitch_setpoint = (input[1]-THROTTLE_RMID+10)/2.0;
+  else if(input[1] < THROTTLE_RMID-10)pid_pitch_setpoint = (input[1]-THROTTLE_RMID-10)/2.0;
+  
+  pitch_error = pitch_speed - pid_pitch_setpoint;
+
+  Proportional_pitch = PITCH_PID_KP * pitch_error;
+  Integral_pitch += PITCH_PID_KI * pitch_error;
+  if(Integral_pitch > PITCH_PID_MAX)Integral_pitch = PITCH_PID_MAX;
+  else if(Integral_pitch < PITCH_PID_MIN)Integral_pitch = PITCH_PID_MIN;
+  Derivative_pitch = PITCH_PID_KD * (pitch_error - last_pitch_error);
+
+  pid_picth_out = Proportional_pitch + Integral_pitch + Derivative_pitch;
+  if(pid_pitch_out > PITCH_PID_MAX)pid_pitch_out = PITCH_PID_MAX;
+  else if(pid_pitch_out < PITCH_PID_MIN)pid_pitch_out = PITCH_PID_MIN;
+
+  last_pitch_error = pitch_error;
+
+//YAW calculations
+  pid_yaw_setpoint = 0;
+  if(input[3] > THROTTLE_RMID+10)pid_yaw_setpoint = (input[3]-THROTTLE_RMID+10)/2.0;
+  else if(input[3] < THROTTLE_RMID-10)pid_yaw_setpoint = (input[3]-THROTTLE_RMID-10)/2.0;
+
+  yaw_error = yaw_speed - pid_yaw_setpoint;
+  
+  Proportional_yaw = YAW_PID_KP * yaw_error;
+  Integral_yaw += YAW_PID_KI * yaw_error;
+  if(Integral_yaw > YAW_PID_MAX)Integral_yaw = YAW_PID_MAX;
+  else if(Integral_yaw < YAW_PID_MIN)Integral_yaw = YAW_PID_MIN;
+  Derivative_yaw = YAW_PID_KD * (yaw_error - last_yaw_error);
+
+  pid_yaw_out = Proportional_yaw + Integral_yaw + Derivative_yaw;
+  if(pid_yaw_out > YAW_PID_MAX)pid_yaw_out = YAW_PID_MAX;
+  else if(pid_yaw_out < YAW_PID_MIN)pid_yaw_out = YAW_PID_MIN;
+
+  last_yaw_error = yaw_error;
+
+ /////////AUTO STABILIZATION MANQUANTE
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
 
 // Fonction qui empeche les moteurs de tourner
 void motors_set_to_zero() {

  Serial.println(pid_yaw_setpoint);
}
void print_imu() {
  Serial.print(pid_roll_speed_in);
  Serial.print(",");
  Serial.print(pid_pitch_speed_in);
  Serial.print(",");
  Serial.println(pid_yaw_speed_in);
}

void print_rx() {

  Serial.println(input[3]);
}

