MANOBRANDO ROBÔ BIMOTOR MEGA-01 VIA BLUETOOTH

#include <MPU6050_tockn.h>
#include <Wire.h>
#include <NewPing.h>
#define TRIGGER_PIN  23  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     22  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 60 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.


MPU6050 mpu6050(Wire);

long timer = 0;
int direcao = 0;
int corretor = 0;
int velDir = 0;
int velEsq = 0;
int angDist = 900;
int comando = 0;

int pot = 100;
int angulo = 0; // angulo de correção máximo em graus;
int distMin = 20; // distância minima até um objeto frontal;

void setup() {

  pinMode(5, OUTPUT); // liga motor da esquerda do carrinho para tras;
  pinMode(2, OUTPUT); // liga motor da direita do carrinho para frente;
  pinMode(4, OUTPUT); // liga motor da esquerda do carrinho para frente;
  pinMode(3, OUTPUT); // liga motor da direita do carrinho para tras;

  Serial.begin(115200); // inicia uma troca de dados na velocidade de 9600 bits/s entre a placa e o pc;
  Wire.begin();      // relacionado com a preparação das portas sda(A4) e scl(A5);
  mpu6050.begin();   // inicia o sensor de mpu 6050;
  mpu6050.calcGyroOffsets(true);
}

void loop() {

  // para a esquerda o valor do sensor de movimento  aumenta;
  // para a direita o valor do sensor de movimento  diminui;

  entradaSerial();
  manobra();
  while (comando == '5') {
    entradaSerial();
    if (millis() - timer > 100) {
      Serial.print("Ping: ");
      Serial.print(sonar.ping_cm()); // Send ping, get distance in cm and print result (0 = outside set distance range)
      Serial.print("cm  ----- ");
      Serial.print(" angulo de correção: ");
      Serial.print(angulo);
      Serial.print(" ----- direcao : ");
      Serial.print(direcao);
      Serial.print(" ----- angulo atual: ");
      Serial.println(mpu6050.getAngleZ());
      diretor(); // função responsavel por controlar os motores mantendo o curso em linha reta;
      conversor(); // função responsavel por fazer o robô parar diante de um obstáculo e fazer uma conversão para direita ou esquerda;
    }
  }
}
void diretor() {  // esta função faz com que o robô corrija sua direção andando sempre em linha reta;
  mpu6050.update();
  direcao = ((mpu6050.getAngleZ()) * 10) + angulo;
  timer = millis();

  if ( direcao >= angulo) { // atualiza velocidade do motor direito;
    velDir = abs(direcao);
    velEsq = 0;
    if (velDir > 100) {
      velDir = 100;
    }
  }

  if ( direcao <= angulo) { // atualiza velocidade do motor esquerdo;
    velEsq = abs(direcao);
    velDir = 0;
    if (velEsq > 100) {
      velEsq = 100;
    }
  }
  // realiza o ajusta da direção do robô controlando as velocidades dos motores;
  velDir = map(velDir, 0, 100, pot, 0);
  analogWrite(2, velDir);
  velEsq = map(velEsq, 0, 100, pot, 0);
  analogWrite(4, velEsq);
}

void conversor() {
  if ( sonar.ping_cm() <= distMin && sonar.ping_cm() != 0) { // desliga os motores para evitar colisão;
    analogWrite(5, 0);
    analogWrite(2, 0);
    analogWrite(4, 0);
    analogWrite(3, 0);
    delay(500);
    angulo += angDist; // soma um valor na nova direção do robô;
    if (angulo >= 3600) {
      angulo = 0;
    }

    delay(1000); // faz com que ao encontrar um obstaculo o robo ande em marcha ré por 1 segundo.
    analogWrite(5, pot);
    analogWrite(3, pot);
    delay(1000);
    analogWrite(5, 0);
    analogWrite(3, 0);
  }
}
void manobra() {
  if (comando == '0') // para o robô;
  {
    analogWrite(5, 0);
    analogWrite(2, 0);
    analogWrite(4, 0);
    analogWrite(3, 0);
    Serial.println("robo parado !!!!!");
  }
  if (comando == '8') // robô anda para frente;
  {
    analogWrite(5, 0);
    analogWrite(2, 150);
    analogWrite(4, 150);
    analogWrite(3, 0);
    Serial.println("robo andando para frente !!!!!");
  }
  if (comando == '2') // robô anda para trás;
  {
    analogWrite(5, 150);
    analogWrite(2, 0);
    analogWrite(4, 0);
    analogWrite(3, 150);
    Serial.println("robo andando para trás !!!!!");
  }
  if (comando == '4') // robô anda para esquerda;
  {
    analogWrite(5, 0);
    analogWrite(2, 0);
    analogWrite(4, 150);
    analogWrite(3, 0);
    Serial.println("robo andando para esquerda !!!!!");
  }
  if (comando == '6') // robô anda para direita;
  {
    analogWrite(5, 0);
    analogWrite(2, 150);
    analogWrite(4, 0);
    analogWrite(3, 0);
    Serial.println("robo andando para direita !!!!!");
  }
}
void entradaSerial() {
  if (Serial.available()) // verifica se existe algo digitado no monitor serial;
  {
    comando = Serial.read(); // carrega o valor digitado no monitor serial na variavel "comando";
  }
}