   pinMode(CH4, INPUT);
   pinMode(CH5, INPUT);
   pinMode(CH6, INPUT);
-  
+
   /* Bind dos canais de saída dos motores (ESCs) */
-  motorFL.attach(MOTOR_FL);
-  motorFR.attach(MOTOR_FR);
-  motorBL.attach(MOTOR_BL);
-  motorBR.attach(MOTOR_BR);
-
-  setpoint = 1500;
-
-  myPID.SetOutputLimits(-255, 255);
-  myPID.SetMode(AUTOMATIC);
+  motorFL.attach(MOTOR_PIN_FL);
+  motorFR.attach(MOTOR_PIN_FR);
+  motorBL.attach(MOTOR_PIN_BL);
+  motorBR.attach(MOTOR_PIN_BR);
+
+  /* Inicializa o PID */
+  pid_pitch_stab.SetMode(AUTOMATIC);
+  pid_roll_stab.SetMode(AUTOMATIC);
+  pid_yaw_stab.SetMode(AUTOMATIC);
+
+  /* Limita o quanto será enviado ao motor: range máximo de 1000ms. */
+  pid_pitch_stab.SetOutputLimits(-500, 500);
+  pid_roll_stab.SetOutputLimits(-500, 500);
+  pid_yaw_stab.SetOutputLimits(-500, 500);
 
   /* Inicializa o barramento I2C (a biblioteca I2Cdev não faz isso automaticamente) */
   Wire.begin();
   TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
 
-  /* Inicializa a porta serial */
-  Serial.begin(115200);
-  while (Serial.available() && Serial.read()); // Limpa o buffer
-
   /* Inicializa a MPU e seu respectivo DMP */
   mpu.initialize();
-  Serial.println(F("Inicializando DMP..."));
+  D(Serial.println(F("Inicializando DMP...")));
   dmpStatus = mpu.dmpInitialize();
 
   /* Define os offsets do MPU. Utilizado o programa MPU6050_calibration para chegar a esses números. */


  readOrientation();

  // readRC();
  
  //  input = ch1v;
  // myPID.Compute();
  // ch1v += (int)output;

  writeMotor();

  // D(telemetry());

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

}

/* Loop infinito do Arduino. */
void loop() {
  static float yaw_target = 0;

  /* Se a programação do DMP falhou, não faz nada */
  if (!dmpReady) {
    return;
