Novo aprimoramento do robô autônomo

Adicionei mais 2 sensores no robô autônomo que se desvia de obstáculos.

Abaixo estão os links para os posts sobre este mesmo projeto.

Parte 1Clique aqui

Parte 2Clique aqui

Os sensores adicionais

Simplesmente, acrescentei mais duas chaves micro switches.

O código abaixo.

#include <NewPing.h>
#include <Servo.h>
#define trigPin 9
#define echoPin 10
#define trigPin2 12
#define echoPin2 11

Servo servo;
int posservo;
 
int IN1=4;
int IN2=5;
int IN3=7;
int IN4=2;
int ENA=3;
int ENB=6;
const int buttonE=0;
int statebuttonE=0;
const int buttonD=5;
int statebuttonD=0;

#define max_distance 200
boolean goesForward = false;
int distance;
 
NewPing sensor(trigPin,echoPin,max_distance);
  
void setup() {
  servo.attach(8);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  pinMode(buttonE,INPUT);
  pinMode(buttonD,INPUT);
  pinMode(echoPin,INPUT);
  pinMode(trigPin,OUTPUT);
  pinMode(echoPin2,INPUT);
  pinMode(trigPin2,OUTPUT);
  Serial.begin(9600);
  servo.write(90);
  delay(150);
}
void loop(){
 analogWrite(ENA,210);
 analogWrite(ENB,210);
 statebuttonE=analogRead(buttonE);
 statebuttonD=analogRead(buttonD);
 int distanceR=0;
 int distanceL=0;
 distance = readPing();
 Serial.println(distance);
 Serial.print("\n");
 if(distance <= 20){//Verifica se há obstáculo à frente.
    moveStop();
    delay(300);
    moveBackward();
    delay(400);
    moveStop();
    delay(300);
    distanceR = lookRight();
    delay(300);
    distanceL = lookLeft();
    delay(300);
    if (distanceR >= distanceL){
      turnRight();
      moveStop();
    }
    else{
      turnLeft();
      moveStop();
    }  
 }else{
  moveForward();
 }
  digitalWrite(trigPin2,LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2,HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2,LOW);
  long duration=pulseIn(echoPin2,HIGH);
  long distance2=duration/29/2;
  Serial.println(distance2);
  if(distance2>10){
    moveBackward();
    delay(1000);
    turnRight();
    delay(1000);
  }

  if(statebuttonE>=500){
    moveBackward();
    delay(500);
    turnRight();
  }
  if(statebuttonD>=500){
    moveBackward();
    delay(500);
    turnLeft();
  }

}
int lookRight(){  
  servo.write(0);
  delay(150);
  int distance = readPing();
  delay(100);
  servo.write(90);
  return distance;
}
int lookLeft(){
  servo.write(180);
  delay(150);
  int distance = readPing();
  delay(100);
  servo.write(90);
  return distance;
  delay(100);
}
int readPing(){
  delay(70);
  int cm = sensor.ping_cm();
  if (cm==0){
    cm=250;
  }
  return cm;
}
void moveStop(){  
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}
void turnRight(){
   digitalWrite(IN1,HIGH);
   digitalWrite(IN2,LOW);
   digitalWrite(IN3,HIGH);
   digitalWrite(IN4,LOW);
   delay(600);  //Valor deve mudar dependendo da carga da bateria.
   digitalWrite(IN1,LOW);
   digitalWrite(IN2,HIGH);
   digitalWrite(IN3,HIGH);
   digitalWrite(IN4,LOW);
}
void turnLeft(){
   digitalWrite(IN1,LOW);
   digitalWrite(IN2,HIGH);
   digitalWrite(IN3,LOW);
   digitalWrite(IN4,HIGH);
   delay(600);  //Valor deve mudar dependendo da carga da bateria.
   digitalWrite(IN1,LOW);
   digitalWrite(IN2,HIGH);
   digitalWrite(IN3,HIGH);
   digitalWrite(IN4,LOW);
}
void moveForward(){
  if(!goesForward){
    goesForward=true;
    digitalWrite(IN1,LOW);
    digitalWrite(IN2,HIGH);
    digitalWrite(IN3,HIGH);
    digitalWrite(IN4,LOW);
    }
}
void moveBackward(){
  goesForward=false;
  digitalWrite(IN1,HIGH);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,HIGH);
}

O vídeo de demonstração.