Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #define IZQTRIGGER 2
- #define IZQECHO 3
- #define DERTRIGGER 4
- #define DERECHO 5
- #define ADETRIGGER 6
- #define ADEECHO 7
- int ENA = 19;
- int IN1 = 18;
- int IN2 = 17;
- int ENB = 14;
- int IN3 = 16;
- int IN4 = 15;
- const float sonido = 34300.0; // Velocidad del sonido en cm/s
- void setup ()
- {
- Serial.begin(9600);
- pinMode (ENA, OUTPUT);
- pinMode (ENB, OUTPUT);
- pinMode (IN1, OUTPUT);
- pinMode (IN2, OUTPUT);
- pinMode (IN3, OUTPUT);
- pinMode (IN4, OUTPUT);
- pinMode (DERECHO, INPUT);
- pinMode (DERTRIGGER, OUTPUT);
- pinMode (IZQECHO, INPUT);
- pinMode (IZQTRIGGER, OUTPUT);
- pinMode (ADEECHO, INPUT);
- pinMode (ADETRIGGER, OUTPUT);
- }
- void loop() {
- //delay (5000);
- //while (true){
- izqIniciarTrigger();
- float distanciaizq = calcularDistanciaIzq();
- derIniciarTrigger();
- float distanciader = calcularDistanciaDer();
- adeIniciarTrigger();
- float distanciaade = calcularDistanciaAde();
- if (distanciaade < 10){
- Adelante();
- Serial.println ("RUUUUUUUUUUUUUN");
- } else {
- Parar();
- Serial.println ("bip");
- }
- //}
- }
- void izqIniciarTrigger()
- {
- // Ponemos el Triiger en estado bajo y esperamos 2 ms
- digitalWrite(IZQTRIGGER, LOW);
- //delayMicroseconds(1);
- // Ponemos el pin Trigger a estado alto y esperamos 10 ms
- digitalWrite(IZQTRIGGER, HIGH);
- //delayMicroseconds(2);
- // Comenzamos poniendo el pin Trigger en estado bajo
- digitalWrite(IZQTRIGGER, LOW);
- }
- void derIniciarTrigger()
- {
- digitalWrite(DERTRIGGER, LOW);
- //delayMicroseconds(1);
- digitalWrite(DERTRIGGER, HIGH);
- //delayMicroseconds(2);
- digitalWrite(DERTRIGGER, LOW);
- }
- void adeIniciarTrigger()
- {
- digitalWrite(ADETRIGGER, LOW);
- //delayMicroseconds(1);
- digitalWrite(ADETRIGGER, HIGH);
- //delayMicroseconds(2);
- digitalWrite(ADETRIGGER, LOW);
- }
- float calcularDistanciaIzq()
- {
- unsigned long tiempo = pulseIn(IZQECHO, HIGH);
- float distancia1 = tiempo * 0.000001 * sonido / 2.0;
- Serial.print(distancia1);
- Serial.print("cm");
- Serial.println();
- return distancia1;
- }
- float calcularDistanciaDer()
- {
- unsigned long tiempo = pulseIn(DERECHO, HIGH);
- float distancia2 = tiempo * 0.000001 * sonido / 2.0;
- Serial.print(distancia2);
- Serial.print("cm");
- Serial.println();
- return distancia2;
- }
- float calcularDistanciaAde()
- {
- // La función pulseIn obtiene el tiempo que tarda en cambiar entre estados, en este caso a HIGH
- unsigned long tiempo = pulseIn(ADEECHO, HIGH);
- // Obtenemos la distancia en cm, hay que convertir el tiempo en segudos ya que está en microsegundos
- // por eso se multiplica por 0.000001
- float distancia3 = tiempo * 0.000001 * sonido / 2.0;
- Serial.print(distancia3);
- Serial.print("cm ade");
- Serial.println();
- return distancia3;
- }
- void Adelante ()
- {
- //Direccion motor A
- digitalWrite (IN1, HIGH);
- digitalWrite (IN2, LOW);
- analogWrite (ENA, 255); //Velocidad motor A
- //Direccion motor B
- digitalWrite (IN3, HIGH);
- digitalWrite (IN4, LOW);
- analogWrite (ENB, 255); //Velocidad motor B
- }
- void Atras ()
- {
- //Direccion motor A
- digitalWrite (IN1, LOW);
- digitalWrite (IN2, HIGH);
- analogWrite (ENA, 128); //Velocidad motor A
- //Direccion motor B
- digitalWrite (IN3, LOW);
- digitalWrite (IN4, HIGH);
- analogWrite (ENB, 128); //Velocidad motor B
- }
- void Derecha ()
- {
- //Direccion motor A
- digitalWrite (IN1, HIGH);
- digitalWrite (IN2, LOW);
- analogWrite (ENA, 200); //Velocidad motor A
- //Direccion motor B
- digitalWrite (IN3, LOW);
- digitalWrite (IN4, HIGH);
- analogWrite (ENB, 100); //Velocidad motor A
- }
- void Izquierda ()
- {
- //Direccion motor A
- digitalWrite (IN1, LOW);
- digitalWrite (IN2, HIGH);
- analogWrite (ENA, 50); //Velocidad motor A
- //Direccion motor B
- digitalWrite (IN3, HIGH);
- digitalWrite (IN4, LOW);
- analogWrite (ENB, 150); //Velocidad motor A
- }
- void Parar ()
- {
- //Direccion motor A
- digitalWrite (IN1, LOW);
- digitalWrite (IN2, LOW);
- analogWrite (ENA, 0); //Velocidad motor A
- //Direccion motor B
- digitalWrite (IN3, LOW);
- digitalWrite (IN4, LOW);
- analogWrite (ENB, 0); //Velocidad motor A
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement