Robot Evasor de obstáculos

 Código Robot Evasor de obstáculos

// Incluimos librería e instanciamos el objeto servo

#include <Servo.h>

Servo cuello_servo;

// Definición pines EnA y EnB para el control de la velocidad

int VelocidadMotor1 = 5; 

int VelocidadMotor2 = 6;

// Definición pines sensor distancia y variables para el cálculo de la distancia

int echoPin = 2; 

int trigPin = 3; 

long duration; 

int distancia; 

//int delayVal;

// Definición de los pines de control de giro de los motores In1, In2, In3 e In4

int Motor1A = 13; 

int Motor1B = 12;  

int Motor2C = 8; 

int Motor2D = 10; 

// Variable control distancia de servos en sus 3 angulos

int servoPos = 0;

int servoReadIzq = 0;

int servoReadDer = 0;

// Configuración de pines

void setup() {

  delay(1000);

  // Vinculamos el pin digital 9 al servo instanciado arriba

  cuello_servo.attach(9);

  // Establecemos modo de los pines del sensor de ultrasonidos

  pinMode(trigPin, OUTPUT); 

  pinMode(echoPin, INPUT); 

  // Establecemos modo de los pines del control de motores

  pinMode(Motor1A,OUTPUT);

  pinMode(Motor1B,OUTPUT);

  pinMode(Motor2C,OUTPUT);

  pinMode(Motor2D,OUTPUT);

  //pinMode(VelocidadMotor1, OUTPUT);

  //pinMode(VelocidadMotor2, OUTPUT);

  // Configuramos velocidad de los dos motores

  analogWrite(VelocidadMotor1, 150); 

  analogWrite(VelocidadMotor2, 150);  

  cuello_servo.write(90); 

  Serial.begin(9600);

}

// Ejecución contínua

void loop() {

  delay(50); 

  distancia = medirDistancia();

  Serial.print("D =  ");

  Serial.print(distancia);

  Serial.println(" cm");

  if(distancia < 15){

    altoCar();

    

    // Miramos a la derecha    

    cuello_servo.write(10);  

    delay(800); 

    servoReadDer = medirDistancia();

    // Miramos a la izquierda                             

    cuello_servo.write(170);  

    delay(800); 

    servoReadIzq = medirDistancia();

    // Miramos de frente 

    cuello_servo.write(90); // Alfrente el servo está a 90º 

    delay(800); 

    if(servoReadIzq > servoReadDer){

      Serial.println("Giro izquierda");

      giraIzqCar();

    }

    if(servoReadDer >= servoReadIzq){

      Serial.println("Giro derecha");

      giraDerCar(); 

    }

  }

  

  if(distancia > 15){

    Serial.println("Recto");

    irFrenteCar();

  }

  

}

void altoCar(){

  // Paramos el carrito

  digitalWrite(Motor1A, LOW);

  digitalWrite(Motor2D, LOW);                       

  digitalWrite(Motor1A, LOW);

  digitalWrite(Motor2D,LOW); 

}

void giraDerCar(){

  // Configuramos sentido de giro para dirar a la derecha

  digitalWrite(Motor1A, LOW);

  digitalWrite(Motor2D,LOW);

  digitalWrite(Motor1A, HIGH);

  digitalWrite(Motor2D,LOW);

  delay(250);

}

void giraIzqCar(){

  // Configuramos sentido de giro para dirar a la izquierda

  digitalWrite(Motor1A,LOW);

  digitalWrite(Motor2D, LOW);

  digitalWrite(Motor1A,LOW);

  digitalWrite(Motor2D, HIGH);

  delay(250);

}

void irFrenteCar(){

  // Configuramos sentido de giro para avanzar

  digitalWrite(Motor1A, LOW);

  digitalWrite(Motor2D, LOW);                       

  digitalWrite(Motor1A, HIGH);

  digitalWrite(Motor2D,HIGH); 

}

int medirDistancia(){

  // Lanzamos pulso de sonido

  digitalWrite(trigPin, LOW);

  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  

  // Leemos lo que tarda el pulso en llegar al sensor y calculamos distancia

  duration = pulseIn(echoPin, HIGH);

  distancia = duration * 0.034 / 2;

  // Devolver distancia calculada

  return distancia;    

}