 
 
 
+


  // D(telemetry());

  // blink LED to indicate activity
  blinkState = !blinkState;
  digitalWrite(LED_PIN, blinkState);
}

PID myPID(&input, &output, &setpoint,1,2,3, DIRECT);

/* Inicialização do Arduino. */
void setup() {
  /* Bind dos canais de entrada do controle */
  pinMode(CH1, INPUT);
  pinMode(CH2, INPUT);
  pinMode(CH3, INPUT);
  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);

  /* 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..."));
  dmpStatus = mpu.dmpInitialize();

  /* Define os offsets do MPU. Utilizado o programa MPU6050_calibration para chegar a esses números. */
  mpu.setXAccelOffset(616);
  mpu.setYAccelOffset(2244);
  mpu.setZAccelOffset(300);
