9º Evitar obstáculos - solución

Evitar Obstáculos solución

#include <DRV8833.h>  

DRV8833 driver = DRV8833();  

int pinUltrasonido = 4;  

int distancia;  

int motorA1 = 5 ;  

int motorA2 = 6;  

int motorB1 = 11;  

int motorB2 = 3;  

int velocidadMotorA = 105;  

int velocidadMotorB = 92;  

void setup() {  

  driver.attachMotorA(motorA1, motorA2);  

  driver.attachMotorB(motorB1, motorB2);  

}

void loop() {  

  int distancia = obtenerDistancia();  

  robotAdelante();  

  if (distancia > 6 && distancia < 15) {  

    robotFrena();  

    delay(1000);  

    robotRetrocede();  

    delay(700);  

    robotDerecha();  

    delay(900);  

  }  

}

void robotAdelante(){  

  driver.motorAForward(velocidadMotorA);  

  driver.motorBForward(velocidadMotorB);  

}  

void robotFrena() {  

  driver.motorAStop();  

  driver.motorBStop();  

}  

void robotRetrocede(){  

  driver.motorBReverse(velocidadMotorB);  

  driver.motorAReverse(velocidadMotorB);  

}  

void robotIzquierda(){  

  driver.motorBForward(velocidadMotorB);  

  driver.motorAStop();  

}  

void robotDerecha(){  

  driver.motorAForward(velocidadMotorB);  

  driver.motorBStop();  

}  

int obtenerDistancia()  

{  

    int d;  

    pinMode(pinUltrasonido, OUTPUT);  

    digitalWrite(pinUltrasonido, LOW);  

    delayMicroseconds(2);  

    digitalWrite(pinUltrasonido, HIGH);  

    delayMicroseconds(10);  

    digitalWrite(pinUltrasonido, LOW);  

    pinMode(pinUltrasonido, INPUT);  

    d = pulseIn(pinUltrasonido, HIGH);  

    d = d / 58;  

    return d;  

}