 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);

#include <Servo.h>
#include <PID_v1.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

void telemetry();
void readRC();
void normalizeRC();

#define DEBUG // Habilita a telemetria
#ifdef DEBUG

      // Reseta a FIFO
      mpu.resetFIFO();
      fifoOverflow = true;
    }

    /* Se houve overflow, aguarda o próximo valor estar pronto */
    if (fifoOverflow) {
      while(!mpuInterrupt);
    }

    if (mpuIntStatus & 0x02) {
      /* Obtém a contagem da FIFO */
      fifoCount = mpu.getFIFOCount();

      /* Aguarda encher a FIFO caso não esteja com os todos dados completos (pacote completo) */
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

      /* Dado está pronto para ser lido do MPU */
      mpu.getFIFOBytes(fifoBuffer, packetSize);

      /* Obtém os valores (Yaw, Pitch e Roll) do DMP */
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
    }
  }
}

/*
 * Efetua a telemetria dos dados do quadricoptero na seguinte ordem, separados por ';':
 * - rcThrottle (em us - entre ~1000 a ~2000)
 * - rcYaw    (entre -150 e 150)
