/*

Este cdigo controla un coche y avanza siguiendo una linea evitando Obstaculos qu
e esten en la linea

Realiza la medicion de distancias con el sensor HC-SR04

Para el movimiento del coche emplea las salidas digitales 5 y 6 en formato PWM
y la librera Servo.h

Los sensores DerechaDerecha e IzquierdaIzquierda no se utilizan en este ejemplo

Este ejemplo es de dominio pblico.
*/

//Importa la libreria Servo.h
#include <Servo.h>
Servo miservo1; // Crea un objeto se
rvo
Servo miservo2;
// Establece las constantes a utilizar
const int centroServo = 95; // Establece los gra
dos que centra al servo
const int velocidad = 15; // Establece la velo
cidad de giro
const int pinServo1=5; // Establece el pin
5 como salida del objeto miservo1
const int pinServo2=6; // Establece el pin
6 como salida del objeto miservo2
const int pinSensorDerecha=4; // Establece el pin
A4 como entrada del sensor derecho
const int pinSensorIzquierda=3; // Establece el pin
A3 como entrada del sensor izquierdo
const int pinSensorDerechaDerecha=5; // Establece el pin
A5 como entrada del sensor derecho-derecho
const int pinSensorIzquierdaIzquierda=2; // Establece el pin
A2 como entrada del sensor izquierdo-izquierdo
const int blanco=300; // Establece el umbr
al mximo de blanco
const int negro=700; // Establece el umbr
al mmino de negro
const int ECHO=12; // Pin para recibir
el pulso de eco
const int TRIGGER=11; // Pin para enviar e
l pulso de disparo
const int cerca=7; // Distancia a la cu
al esquiva el obstaculo. maximo 15 cm
const int giro=700;
const int pasos=1100;
const int duraccionMaxPulso=1000; // Tiempo maximo de
espera en microsegundos
// Establecemos las variables a utilizar
volatile unsigned distancia;
int sensorDerecha,sensorIzquierda,sensorVal3,sensorVal4; // Establece la var
iable que almacena la lectura del Sensor
String posicion,posicionSensorDerecha,posicionSensorIzquierda,posicionSensorDere
chaDerecha,posicionSensorIzquierdaIzquierda;
void setup()
{
miservo1.attach(pinServo1); // Atribuye el pin a
l objeto miservo1
miservo2.attach(pinServo2); // Atribuye el pin a
l objeto miservo2
pinMode(ECHO, INPUT); // Establece ECHO co
mo pin de entrada digital
pinMode(TRIGGER, OUTPUT); // Establece TRIGGER
como pin de salida digital
}
void loop() {
medir ();
if (distancia<cerca)
{
esquiva();
}
else
{
// nada
}

sensorDerecha = analogRead(pinSensorDerecha); // Lee el valor del
sensor 1 valores entre 0 y 1023
sensorIzquierda = analogRead(pinSensorIzquierda); // Lee el valor del
sensor 2 valores entre 0 y 1023
sensorVal3 = analogRead(pinSensorDerechaDerecha); // Lee el valor del
sensor 3 No utilizado
sensorVal4 = analogRead(pinSensorIzquierdaIzquierda); // Lee el valor del
sensor 4 No utilizado
posicion=""; // Establece cadena
vacia
posicionSensorDerechaDerecha="X"; // Asigna "X" a la v
ariable posicionSensorDerechaDerecha
posicionSensorIzquierdaIzquierda="X"; // Asigna "X" a la v
ariable posicionSensorIzquierdaIzquierda

// Identifica posicin del sensor Derecho
if (sensorDerecha> negro)
{
posicionSensorDerecha="1";
}
else if (sensorDerecha< blanco)
{
posicionSensorDerecha="0";
}
// Identifica posicin del sensor Izquierdo
if (sensorIzquierda> negro)
{
posicionSensorIzquierda="1";
}
else if (sensorIzquierda< blanco)
{
posicionSensorIzquierda="0";
}
// Concatena las variables parciales en la variable posicion
posicion.concat(posicionSensorIzquierdaIzquierda);
posicion.concat(posicionSensorIzquierda);
posicion.concat(posicionSensorDerecha);
posicion.concat(posicionSensorDerechaDerecha);
// Dependiendo del valor de la variable posicion, realiza una accin
if (posicion=="X11X"){avanza();}
if (posicion=="X10X"){izquierda();}
if (posicion=="X01X"){derecha();}
if (posicion=="X00X"){/* Sigue con lo que estabas realizando */}
}
void medir ()
{
digitalWrite(TRIGGER, LOW); // Triger en bajo
delayMicroseconds(2); // Esperamos 2 mi
lisegundos
digitalWrite(TRIGGER, HIGH); // Triger en alto

delayMicroseconds(10); // Esperamos 10 m
ilisegundos
digitalWrite(TRIGGER, LOW); // Triger en bajo
distancia = (pulseIn(ECHO, HIGH,duraccionMaxPulso))/29.15/2; // Medimos la dis
tancia basandonos en que el tiempo que
if ( distancia == 0 ) {distancia = duraccionMaxPulso;} // tarda el sonid
o en recorrer 1 cm es de 29 microsegundos
}
void esquiva()
{
derecha();
delay(giro);
avanza();
delay(pasos);
izquierda();
delay(giro);
avanza();
delay(2*pasos);
izquierda();
delay(giro);
avanza();
delay (pasos*2/3);
derecha();
delay(giro*2/3);
avanza();
}
// Para entender las rutinas, hay que tener en cuentra, que los servos se encuen
tran enfrentados
void derecha()
{
miservo1.write(centroServo+velocidad); // Posiciona el servo1 en l
a posicion adelante
miservo2.write(centroServo+velocidad); // Posiciona el servo2 en l
a posicion adelante
}
void izquierda()
{
miservo1.write(centroServo-velocidad); // Posiciona el servo1 en l
a posicion atras
miservo2.write(centroServo-velocidad); // Posiciona el servo2 en l
a posicion atras
}
void avanza()
{
miservo1.write(centroServo-velocidad); // Posiciona el servo1 en la
posicion atras
miservo2.write(centroServo+velocidad); // Posiciona el servo2 en la
posicion adelante
}