28-07-2014, 08:33 PM
Prueba algo así
A ver si hay suerte...
Código:
#include <Servo.h>
long distancia1;
long distancia2;
long distancia3;
long tiempo;
Servo servo;
int IN3 = 5;
int IN4 = 4;
int IN1 = 3;
int IN2 = 6;
int movimiento = false;
void setup(){
Serial.begin(9600);
pinMode(9, OUTPUT); /*activación del pin 9 como salida: para el pulso ultrasónico*/
pinMode(8, INPUT); /*activación del pin 8 como entrada: tiempo del rebote del ultrasonido*/
servo.attach(10);
pinMode (IN4, OUTPUT); // Input4 conectada al pin 4
pinMode (IN3, OUTPUT); // Input3 conectada al pin 5
pinMode (IN2, OUTPUT); // Input2 conectada al pin 6
pinMode (IN1, OUTPUT); // Input1 conectada al pin 3
}
void loop(){
servo.write(0);
delay(250);
digitalWrite(9,LOW); /* Por cuestión de estabilización del sensor*/
delayMicroseconds(5);
digitalWrite(9, HIGH); /* envío del pulso ultrasónico*/
delayMicroseconds(10);
tiempo=pulseIn(8, HIGH); /* Función para medir la longitud del pulso entrante. Mide el tiempo que transcurrido entre el envío
del pulso ultrasónico y cuando el sensor recibe el rebote, es decir: desde que el pin 12 empieza a recibir el rebote, HIGH, hasta que
deja de hacerlo, LOW, la longitud del pulso entrante*/
distancia1= int(0.017*tiempo); /*fórmula para calcular la distancia obteniendo un valor entero*/
/*Monitorización en centímetros por el monitor serial*/
Serial.print("Distancia ");
Serial.print(distancia1);
Serial.println(" cm");
delay(750);
servo.write(90);
delay(250);
digitalWrite(9,LOW); /* Por cuestión de estabilización del sensor*/
delayMicroseconds(5);
digitalWrite(9, HIGH); /* envío del pulso ultrasónico*/
delayMicroseconds(10);
tiempo=pulseIn(8, HIGH); /* Función para medir la longitud del pulso entrante. Mide el tiempo que transcurrido entre el envío
del pulso ultrasónico y cuando el sensor recibe el rebote, es decir: desde que el pin 12 empieza a recibir el rebote, HIGH, hasta que
deja de hacerlo, LOW, la longitud del pulso entrante*/
distancia2= int(0.017*tiempo); /*fórmula para calcular la distancia obteniendo un valor entero*/
/*Monitorización en centímetros por el monitor serial*/
Serial.print("Distancia ");
Serial.print(distancia2);
Serial.println(" cm");
delay(750);
servo.write(180);
delay(250);
digitalWrite(9,LOW); /* Por cuestión de estabilización del sensor*/
delayMicroseconds(5);
digitalWrite(9, HIGH); /* envío del pulso ultrasónico*/
delayMicroseconds(10);
tiempo=pulseIn(8, HIGH); /* Función para medir la longitud del pulso entrante. Mide el tiempo que transcurrido entre el envío
del pulso ultrasónico y cuando el sensor recibe el rebote, es decir: desde que el pin 12 empieza a recibir el rebote, HIGH, hasta que
deja de hacerlo, LOW, la longitud del pulso entrante*/
distancia3= int(0.017*tiempo); /*fórmula para calcular la distancia obteniendo un valor entero*/
/*Monitorización en centímetros por el monitor serial*/
Serial.print("Distancia ");
Serial.print(distancia3);
Serial.println(" cm");
delay(750);
servo.write(90);
delay(250);
digitalWrite(9,LOW); /* Por cuestión de estabilización del sensor*/
delayMicroseconds(5);
digitalWrite(9, HIGH); /* envío del pulso ultrasónico*/
delayMicroseconds(10);
tiempo=pulseIn(8, HIGH); /* Función para medir la longitud del pulso entrante. Mide el tiempo que transcurrido entre el envío
del pulso ultrasónico y cuando el sensor recibe el rebote, es decir: desde que el pin 12 empieza a recibir el rebote, HIGH, hasta que
deja de hacerlo, LOW, la longitud del pulso entrante*/
distancia2= int(0.017*tiempo); /*fórmula para calcular la distancia obteniendo un valor entero*/
/*Monitorización en centímetros por el monitor serial*/
Serial.print("Distancia ");
Serial.print(distancia2);
Serial.println(" cm");
delay(750);
if (distancia2 >= distancia1 && distancia2 >= distancia3 && movimiento == false) //Va hacia delante
{
digitalWrite (IN4, LOW);
digitalWrite (IN3, HIGH);
digitalWrite (IN2, LOW);
digitalWrite (IN1, HIGH);
movimiento = true;
delay (1000);
movimiento = false;
}
if (distancia1 >= distancia2 && distancia1 >= distancia3 && movimiento == false) //Va hacia la dereha
{
digitalWrite (IN4, HIGH);
digitalWrite (IN3, LOW);
digitalWrite (IN2, LOW);
digitalWrite (IN1, HIGH);
movimiento = true;
delay (500);
movimiento = false;
}
if (distancia3 >= distancia1 && distancia3 >= distancia2 && movimiento == false) // Va hacia la izquierda
{
digitalWrite (IN4, LOW);
digitalWrite (IN3, HIGH);
digitalWrite (IN2, HIGH);
digitalWrite (IN1, LOW);
movimiento = true;
delay(500);
movimiento = false;
}
if (distancia2 <= 5 && movimiento == false)
{
digitalWrite (IN4, HIGH); //Va hacia atrás
digitalWrite (IN3, LOW);
digitalWrite (IN2, HIGH);
digitalWrite (IN1, LOW);
movimiento = true;
delay(500);
movimiento = false;
}
}
A ver si hay suerte...