You are on page 1of 2

1 #define echoPin1 13 // Echo Pin numero 1 derecha

2 #define trigPin1 12 // Trigger Pin numero 1 derecha


1 #define echoPin2 11 // Echo Pin numero 1 derecha
2 #define trigPin2 10 // Trigger Pin numero 1 derecha
1 #define echoPin3 9 // Echo Pin numero 3 izquierda
2 #define trigPin3 8 // Trigger Pin numero 3 izquierda
1 long duration1, distance1; // Duracion 1, usada para calcular distancia 1
2 long duration2, distance2; // Duracion 2, usada para calcular distancia 2
3 long duration3, distance3; // Duracion 3, usada para calcular distancia 3
1 int m1 = 2;//frente
2 int m2 = 5;//atras
3 int m3 = 3;//atras
4 int m4 = 4;//frente
5 void setup() {
6 Serial.begin (9600);
7 pinMode(m1, OUTPUT);
8 pinMode(m2, OUTPUT);
9 pinMode(m3, OUTPUT);
10 pinMode(m4, OUTPUT);
11 //Número 1 Derecha
12 pinMode(trigPin1, OUTPUT);
13 pinMode(echoPin1, INPUT);
14 //Número 2 Centro
15 pinMode(trigPin2, OUTPUT);
16 pinMode(echoPin2, INPUT);
17 //Número 3 Izquierda
18 pinMode(trigPin3, OUTPUT);
19 pinMode(echoPin3, INPUT);
20 }
1 void loop() {
1 //Número 1 Derecha
2 digitalWrite(trigPin1, LOW);
3 delayMicroseconds(2);
4 digitalWrite(trigPin1, HIGH);
5 delayMicroseconds(10);
6 digitalWrite(trigPin1, LOW);
7 duration1 = pulseIn(echoPin1, HIGH);
8 //Calculo de distancia 1 Centro (in cm) basado en la velocidad del sonido.
9 distance1 = duration1/58.2;
1 //Número 2 Centro
2 digitalWrite(trigPin2, LOW);
3 delayMicroseconds(2);
4 digitalWrite(trigPin2, HIGH);
5 delayMicroseconds(10);
6 digitalWrite(trigPin2, LOW);
7 duration2 = pulseIn(echoPin2, HIGH);
8 //Calculo de distancia 2 Centro (in cm) basado en la velocidad del sonido.
9 distance2 = duration2/58.2;
1 //Número 3 Izquierda
2 digitalWrite(trigPin3, LOW);
3 delayMicroseconds(2);
4 digitalWrite(trigPin3, HIGH);
5 delayMicroseconds(10);
6 digitalWrite(trigPin3, LOW);
7 duration3 = pulseIn(echoPin3, HIGH);
8 //Calculo de distancia 3 Izquierda (in cm) basado en la velocidad del sonido.
9 distance3 = duration3/58.2;
1 //impresion de distancias
2 Serial.println("Distacncia 1->");
3 Serial.println(distance1);
4 Serial.println("Distacncia 2->");
5 Serial.println(distance2);
6 Serial.println("Distacncia 3->");
7 Serial.println(distance3);
1 if(distance1>11 && distance2<11 && distance3<11){
2 digitalWrite(m1, HIGH);
3 digitalWrite(m2, LOW);
4 digitalWrite(m3, LOW);
5 digitalWrite(m4, HIGH);
6}
1 if(distance1<11 && distance2<11 && distance3>11){
2 digitalWrite(m1, HIGH);
3 digitalWrite(m2, LOW);
4 digitalWrite(m3, HIGH);
5 digitalWrite(m4, LOW);
1 }
2 //IZQUIERDA
3 if(distance1>11 && distance2<11 && distance3>11){
4 digitalWrite(m1, HIGH);
5 digitalWrite(m2, LOW);
6 digitalWrite(m3, LOW);
7 digitalWrite(m4, HIGH);
8 }
9 //DERECHA
10 if(distance1<11 && distance2<11 && distance3<11){
11 digitalWrite(m1, LOW);
12 digitalWrite(m2, HIGH);
13 digitalWrite(m3, HIGH);
14 digitalWrite(m4, LOW);
15 }
1 delay(100);
2 }

You might also like