   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. */

  mpu.setZAccelOffset(300);
  mpu.setXGyroOffset(83);
  mpu.setYGyroOffset(-25);
  mpu.setZGyroOffset(44); 

  /* Só prossegue somente se o houve a inicialização correta do DMP */
  if (dmpStatus == 0) {

 * - rcRoll   (entre -45 e 45)
 * - rcOnOff  (entre 0 e 100)
 * - rcDimmer (entre 0 e 1000)
 * - Posição: pitch (em graus)
 * - Posição: roll (em graus)
 * - Posição: yaw (em graus)
 * - Saída pitch do PID stab: pitch_stab_output (entre -500 a 500)
 * - Saída roll do PID stab: roll_stab_output (entre -500 a 500)
 * - Saída yaw do PID stab: yaw_stab_output (entre -500 a 500)
 * - Motor FL (em us - entre 1000 a 2000)
 * - Motor FR (em us - entre 1000 a 2000)
 * - Motor BL (em us - entre 1000 a 2000)
 * - Motor BR (em us - entre 1000 a 2000)
 * - Canal 1 (em us - entre ~1000 a ~2000)
 * - Canal 2 (em us - entre ~1000 a ~2000)
 * - Canal 3 (em us - entre ~1000 a ~2000)
 * - Canal 4 (em us - entre ~1000 a ~2000)
 * - Canal 5 (em us - entre ~1000 a ~2000)
 * - Canal 6 (em us - entre ~1000 a ~2000)
 * - Intervalo do loop (em us)
 */
void telemetry() {
  /* Calcula a frequência do loop */
  tmInterval = micros() - tmCheckpoint;
  tmCheckpoint = micros();

  String tmValues;
  tmValues += padding("RCT=" + String(rcThrottle), 8) + ";";
  tmValues += padding("RCY=" + String(rcYaw), 8) + ";";
  tmValues += padding("RCP=" + String(rcPitch), 8) + ";";
  tmValues += padding("RCR=" + String(rcRoll), 8) + ";";
  tmValues += padding("RCO=" + String(rcOnOff), 8) + ";";
  tmValues += padding("RCD=" + String(rcDimmer), 8) + ";";
  tmValues += padding("DMP=" + String(ypr[DMP_PITCH]), 10) + ";";
  tmValues += padding("DMR=" + String(ypr[DMP_ROLL]), 10) + ";";
  tmValues += padding("DMY=" + String(ypr[DMP_YAW]), 10) + ";";
  tmValues += padding("PIP=" + String(pitch_stab_output), 7) + ";";
  tmValues += padding("PIR=" + String(roll_stab_output), 7) + ";";
  tmValues += padding("PIY=" + String(yaw_stab_output), 7) + ";";
  tmValues += padding("MFL=" + String(motors[MOTOR_FL]), 8) + ";";
  tmValues += padding("MFR=" + String(motors[MOTOR_FR]), 8) + ";";
  tmValues += padding("MBL=" + String(motors[MOTOR_BL]), 8) + ";";
  tmValues += padding("MBR=" + String(motors[MOTOR_BR]), 8) + ";";
  tmValues += padding("INT=" + String(tmInterval), 10) + ";";
  // for (int i=0; i < 6; i++) {
  //  tmValues += padding(String(channels[i]), 8) + ";";
  // }

  Serial.println(tmValues);
}

String padding(String text, int size){
  String newString = String(text);
  if (text.length() < size) {
    for (int i = 0; i < size - text.length(); i++) {
      newString += " ";
    }
  }

  return newString;
}