Aprimoramento do robô autônomo

Adicionei mais um sensor no robô autônomo que se desvia de obstáculos.

Post sobre o robô autônomo que evita obstáculosClique aqui

Com mais um sensor de ultrassom, o robô não apenas evita paredes como também evita quedas, detectando bordas.

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;

#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(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,190);
 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);
  }
}
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);
}