Desviando de obstáculos

Ola pessoal. Nesta aula vamos aprender a regular os tempos do nosso carrinho robô, de modo que ele possa ao encontrar um obstáculo a frente, contornar o mesmo e seguir seu caminho. neste programa estamos mudando um dos pinos da ponte H. O fio que estava conectado a porta 3 do arduino, agora deve ser conectado a porta 10. O TRIGGER_PIN  conectado a porta 8  e o ECHO_PIN    conectado a porta 7.

#include <NewPing.h>

//////////////tempo virando e potencia das rodas////////////////
//distancia para parar:
int dist = 40;
//tempo virando:
int tempovirando = 3000;
//tempo a frente:
int tempoafrente = 3000;
//potencia a frente:
int rodaesquerda1 = 255;
int rodadireita1 = 255;
//potencia virando:
int rodaesquerda2 = 155;
int rodadireita2 = 155;
////////////////////////////////////////////////////////////////
int sensorfrente;
int sensoresquerda;
int tempo;
int contornando;
int sensoranterior;
int sensoratual;

#define TRIGGER_PIN  8  // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN     7  // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // 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.

void setup() {
  Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
  pinMode(10, OUTPUT);
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(9, OUTPUT);

}

void loop() {
  delay(50);  // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
  sensorfrente = sonar.ping_cm();
  Serial.print("Ping: ");
  Serial.print(sensorfrente); // Send ping, get distance in cm and print result (0 = outside set distance range)
  Serial.println("cm");
 
  ////////////////////////////controle de decisões e movimentos //////////////////////////////////////////

  if (sensorfrente > dist || sensorfrente == 0) {

    analogWrite(10, rodadireita1);
    analogWrite(5, 0);
    analogWrite(6, rodaesquerda1);
    analogWrite(9, 0);
    sensoratual = sensorfrente;

  }

  if (sensorfrente < dist && sensorfrente != 0) {
   
    // vira para a direita
   
    tempo = millis() + tempovirando;
    while (tempo > millis()) {
      analogWrite(10, rodadireita2);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, rodaesquerda2);
      delay(100);
      analogWrite(10, 0);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, 0);
      delay(100);
    }
   
    // anda para frente
   
    tempo = millis() + tempoafrente;
    while (tempo > millis()) {
      analogWrite(10, rodadireita2);
      analogWrite(5, 0);
      analogWrite(6, rodaesquerda2);
      analogWrite(9, 0);
      delay(100);
      analogWrite(10, 0);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, 0);
      delay(100);
    }
   
    // vira para esquerda
   
    tempo = millis() + tempovirando;
    while (tempo > millis()) {
      analogWrite(5, rodadireita2);
      analogWrite(10, 0);
      analogWrite(6, rodaesquerda2);
      analogWrite(9, 0);
      delay(100);
      analogWrite(10, 0);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, 0);
      delay(100);
    }
   
    // anda para frente
   
     tempo = millis() + tempoafrente;
    while (tempo > millis()) {
      analogWrite(10, rodadireita2);
      analogWrite(5, 0);
      analogWrite(6, rodaesquerda2);
      analogWrite(9, 0);
      delay(100);
      analogWrite(10, 0);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, 0);
      delay(100);
    }
    // vira para esquerda
   
    tempo = millis() + tempovirando;
    while (tempo > millis()) {
      analogWrite(5, rodadireita2);
      analogWrite(10, 0);
      analogWrite(6, rodaesquerda2);
      analogWrite(9, 0);
      delay(100);
      analogWrite(10, 0);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, 0);
      delay(100);
    }
   
    // anda para frente
   
     tempo = millis() + tempoafrente;
    while (tempo > millis()) {
      analogWrite(10, rodadireita2);
      analogWrite(5, 0);
      analogWrite(6, rodaesquerda2);
      analogWrite(9, 0);
      delay(100);
      analogWrite(10, 0);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, 0);
      delay(100);
    }
   
    // vira para a direita
     tempo = millis() + tempovirando;
    while (tempo > millis()) {
      analogWrite(10, rodadireita2);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, rodaesquerda2);
      delay(100);
      analogWrite(10, 0);
      analogWrite(5, 0);
      analogWrite(6, 0);
      analogWrite(9, 0);
      delay(100);
    }
   
  }

/////////////////////////// fim do controle de decisões e movimentos ///////////////////////////////////

}