 Servo motorBL;
 Servo motorBR;
 
-/* Canais de cada motor */
-const int MOTOR_FL = 8; /* To-do test this pin */
-const int MOTOR_FR = 9; 
-const int MOTOR_BL = 10; /* To-do test this pin */
-const int MOTOR_BR = 11; /* To-do test this pin */
-
-/* Parâmetros do PID. */
-double setpoint, input, output;
-
-PID myPID(&input, &output, &setpoint,1,2,3, DIRECT);
+/* Motores (parte lógica) */
+int motors[4];
+const int MOTOR_FL = 0;
+const int MOTOR_FR = 1;
+const int MOTOR_BL = 2;
+const int MOTOR_BR = 3;
 
 /* Inicialização do Arduino. */
 void setup() {
+  /* Inicializa a porta serial */
+  D(Serial.begin(115200));
+  D(while (Serial.available() && Serial.read())); // Limpa o buffer
+
   /* Bind dos canais de entrada do controle */
   pinMode(CH1, INPUT);
   pinMode(CH2, INPUT);

}

/* Loop infinito do Arduino. */
void loop() {       
  /* Se a programação do DMP falhou, não faz nada */
  if (!dmpReady) {
    return;


  readOrientation();

  readRC();

  /* Voe Forest, Voe! */
  if(rcThrottle > MIN_RC_THROTTLE + 100) {  // Throttle raised, turn on stablisation.

    pid_pitch_stab.Compute();
    pid_roll_stab.Compute();
    pid_yaw_stab.Compute();

    // is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
    if(abs(rcYaw ) > 5) {
      yaw_stab_output = rcYaw;
      yaw_target = ypr[DMP_YAW];   // remember this yaw for when pilot stops
    }

    // pid_pitch_rate.calculate()
    // pid_roll_rate.calculate()
    // pid_yaw_rate.calculate()

    //  // Stablise PIDS                                                     'setpoint' 'input'
    // float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
    // float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
    // float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);

    // // is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
    // if(abs(rcyaw ) > 5) {
    //   yaw_stab_output = rcyaw;
    //   yaw_target = yaw;   // remember this yaw for when pilot stops
    // }

    // // rate PIDS                                                       'setpoint'         'input'
    // long pitch_output =  (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);
    // long roll_output =  (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);
    // long yaw_output =  (long) constrain(pids[PID_YAW_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);

    // // mix pid outputs and send to the motors.
    // hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output);
    // hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output);
    // hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output);
    // hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output);

    motors[MOTOR_FL] = rcThrottle + roll_stab_output + pitch_stab_output - yaw_stab_output;
    motors[MOTOR_BL] = rcThrottle + roll_stab_output - pitch_stab_output + yaw_stab_output;
    motors[MOTOR_FR] = rcThrottle - roll_stab_output + pitch_stab_output + yaw_stab_output;
    motors[MOTOR_BR] = rcThrottle - roll_stab_output - pitch_stab_output - yaw_stab_output;

  } else {
    /* Motores desligados */
    motors[MOTOR_FL] = motors[MOTOR_FR] = motors[MOTOR_BL] = motors[MOTOR_BR] = 1000;

    // reset yaw target so we maintain this on takeoff
    yaw_target = ypr[DMP_YAW];

    // reset PID integrals whilst on the ground
    // for(int i=0; i<6; i++)
    //   pids[i].reset_I();

    // }
  }

  writeMotor();

  D(telemetry());

  /* Pisca a led para indicar atividade */
  blinkState = !blinkState;
