robot.ino

//Codigo corrigido
#include <Servo.h> 

Servo myservo;   
int distance = 100;
long distancia;
long tiempo;

void setup() {
  pinMode(7, OUTPUT);
  pinMode(8, OUTPUT);
  
  pinMode(3, OUTPUT);
  pinMode(4, OUTPUT);
  Serial.begin(9600);
  pinMode(10, OUTPUT); //Configuracion del pin 9 como salida: Trigger - Disparo
  pinMode(12, INPUT); //Configuracion del pin 8 como entrada: Recibira el Echo

  myservo.attach(11);  
  myservo.write(115); 
  delay(2000);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
  distance = readPing();
  delay(100);
}

void loop() {
 int distanceR = 0;
 int distanceL =  0;
 Serial.println(distancia);
 if(distance<=30)
 {
  moveStop();
  delay(100);
  moveBackward();
  delay(300);
  moveStop();
  delay(200);
  distanceR = lookRight();
  delay(200);
  distanceL = lookLeft();
  delay(200);

  if(distanceR>=distanceL)
  {
    turnRight();
    delay(300);
    moveStop();
     
  }else
  {
    turnLeft();
    delay(400);
    moveStop();
    Serial.println("Turn Left");
  }
 }else
 {
  moveForward();
  delay(500);
 }
 distance = readPing();
}

int lookRight()
{
    myservo.write(50); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115); 
    return distance;
}

int lookLeft()
{
    myservo.write(170); 
    delay(500);
    int distance = readPing();
    delay(100);
    myservo.write(115); 
    return distance;
    delay(100);
}

void moveStop() {

   //Motor2
  digitalWrite(4, LOW);
  digitalWrite(3, LOW);
  analogWrite(5, 0);
  Serial.println("Move stop");
  digitalWrite(8, LOW);
  digitalWrite(7, LOW);
  analogWrite(6, 0);
  } 
  
void moveForward() {
 //Motor1
  digitalWrite(8, LOW);
  digitalWrite(7, HIGH);
  analogWrite(6, 120);
  digitalWrite(4, LOW);
  digitalWrite(3, HIGH);
  analogWrite(5, 120);
  
  Serial.println("Moving Foward");
}

void moveBackward() {
  digitalWrite(8, HIGH);
  digitalWrite(7, LOW);
  analogWrite(6, 150);
   //Motor2
  digitalWrite(4, HIGH);
  digitalWrite(3, LOW);
  analogWrite(5, 150);
  Serial.println("Moving Backwards");
}  

void turnRight() {
  digitalWrite(8, LOW);
  digitalWrite(7, LOW);
  analogWrite(6, 0);
   //Motor2
  digitalWrite(4, HIGH);
  digitalWrite(3, LOW);
  analogWrite(5, 150);
  Serial.println("Turning Right");
  
  
} 
 
void turnLeft() {
  digitalWrite(8, HIGH);
  digitalWrite(7, LOW);
  analogWrite(6, 150);
   //Motor2
  digitalWrite(4, LOW);
  digitalWrite(3, LOW);
  analogWrite(5, 0);
  Serial.println("Turn Left");
} 

int readPing() {
  digitalWrite(10, LOW); //Para darle estabilización al sensor
  delayMicroseconds(5); //Tiempo de 5 micro segundos
  digitalWrite(10, HIGH); //Enviamos el pulso ultrasónico para activar el sensor
  delayMicroseconds(10); //Con una duracion de 10 micro segundos
  tiempo = pulseIn(12, HIGH); //Función para medir la longitud del pulso entrante, mide el tiempo transcurrido de ida y vuelta
  distancia = int(0.017 * tiempo); //Fórmula para calcular la distancia obteniendo un valor entero
  //Monitorización en centímetros por el monitor serial
 
  //delay(1000); //Cada que Tiempo se imprimira el valor de la distancia
  return distancia;
}