 //PID pid_yaw_rate   (/* taxa de giro */, &yaw_rate_output, &yaw_stab_output, 1, 2, 3, DIRECT);
 
 /* PIDs para a taxa de estabilização. Entrada: posição do quadricóptero através do MPU. Objetivo: Posição indicada através do controle. Saída: Taxa de giro proporcional ao erro. */
-/*                         ENTRADA                         SAÍDA                  OBJETIVO        P   D  I  DIREÇÃO */
-PID pid_pitch_stab ((double *)&ypr[DMP_PITCH], (double *)&pitch_stab_output, (double *)&rcPitch, 0.7, 0, 1, DIRECT);
-PID pid_roll_stab  ((double *)&ypr[DMP_ROLL],  (double *)&roll_stab_output,  (double *)&rcRoll,  0.7, 0, 1, DIRECT);
-PID pid_yaw_stab   ((double *)&ypr[DMP_YAW],   (double *)&yaw_stab_output,   (double *)&rcYaw,   0.7, 0, 1, DIRECT);
+/*                        ENTRADA                 SAÍDA          OBJETIVO  P  I  D  DIREÇÃO */
+PID pid_pitch_stab (&ypr_degree[DMP_PITCH], &pitch_stab_output, &rcPitch, 10, 4, 0, DIRECT);
+PID pid_roll_stab  (&ypr_degree[DMP_ROLL],  &roll_stab_output,  &rcRoll,  10, 4, 0, DIRECT);
+PID pid_yaw_stab   (&ypr_degree[DMP_YAW],   &yaw_stab_output,   &rcYaw,   10, 4, 0, DIRECT);
 /* Fim do PID */
 
 /*************************

 *************************/

/* Canais de cada motor */
const int MOTOR_PIN_FL = 8; /* To-do test this pin */
const int MOTOR_PIN_FR = 9;
const int MOTOR_PIN_BL = 10; /* To-do test this pin */
const int MOTOR_PIN_BR = 11; /* To-do test this pin */

/* Entidades que representam os motores. Responsáveis por acionar os ESCs realizando um PPM entre 1ms e 2ms. */
Servo motorFL;

    motors[MOTOR_BR] = rcThrottle - roll_stab_output - pitch_stab_output - yaw_stab_output;

  } else {
    Serial.println("else");
    /* Motores desligados */
    motors[MOTOR_FL] = motors[MOTOR_FR] = motors[MOTOR_BL] = motors[MOTOR_BR] = 1000;

