Evitando obstáculos (2)

Continuando mis experiencias con el sensor de ultrasonidos, he realizado un sencillo programa que evita chocar contra obstáculos de forma autónoma.

El programa completo desarrollado para tal fin, es el siguiente:

const int DISPARO = 4;
const int ECO     = 5;
const int PWM_I   = 6;
const int MtrI1   = 7;
const int MtrI2   = 8;
const int PWM_D   =11;
const int MtrD1   =12;
const int MtrD2   =13;

byte aux = 0;
unsigned long D = 0;

void setup() {
  pinMode(DISPARO, OUTPUT); 
  pinMode(ECO,     INPUT ); 
  pinMode(PWM_I,   OUTPUT);
  pinMode(MtrI1,   OUTPUT);
  pinMode(MtrI2,   OUTPUT);
  pinMode(PWM_D,   OUTPUT);
  pinMode(MtrD1,   OUTPUT);
  pinMode(MtrD2,   OUTPUT);
  Sentido(4);
  delay(2000);
}

void loop() {
  D = Distancia();
  if (aux > 4) {
    (D>5)? Avanza(): Esquiva();
    analogWrite(PWM_I, 175); 
    analogWrite(PWM_D, 175); 
  } else aux++;
}

unsigned long Distancia() {
  digitalWrite(DISPARO, LOW);
  delayMicroseconds(2);
  digitalWrite(DISPARO, HIGH);
  delayMicroseconds(10);
  digitalWrite(DISPARO, LOW);
  return int(0.017 * pulseIn(ECO, HIGH)); 
}

void Avanza() {
  Sentido(1);
}    

void Esquiva() {

  //Detiene
  Sentido(4); 
  delay(50);
  
  //Retrocede
  Sentido(0);
  delay(300);
  
  //Detiene
  Sentido(4); 
  delay(50);

  //Gira
  Sentido(random(2, 4));
  delay(random(250, 500));
  
  //Detiene
  Sentido(4); 
  delay(50);
}
  
void Sentido(byte d) { 
  switch (d) {
    case 0: //Dirección 1
            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, LOW);  
            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, HIGH); 
            break;
    case 1: //Dirección 2
            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, HIGH); 
            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, LOW);  
            break;
    case 2: //Giro 1
            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, LOW);  
            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, LOW); 
            break;
    case 3: //Giro 2
            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, HIGH); 
            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, HIGH); 
            break;
    case 4: //Detiene (LOW)
            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, LOW);  
            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, LOW);  
            break;
    case 5: //Detiene (HIGH)
            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, HIGH); 
            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, HIGH); 
            break;
  }
}

Las primeras líneas, corresponden a las definiciones de los pines utilizados y no hay nada nuevo:

const int DISPARO = 4;

const int ECO     = 5;

const int PWM_I   = 6;

const int MtrI1   = 7;

const int MtrI2   = 8;

const int PWM_D   =11;

const int MtrD1   =12;

const int MtrD2   =13;

Cómo es tradición, a continuación las variables:

byte aux = 0;

unsigned long D = 0;

Donde "aux" es una variable auxiliar y "D" la utilizo para almacenar la distancia medida.

La función de configuración es la siguiente:

void setup() {

  pinMode(DISPARO, OUTPUT); 

  pinMode(ECO,     INPUT ); 

  pinMode(PWM_I,   OUTPUT);

  pinMode(MtrI1,   OUTPUT);

  pinMode(MtrI2,   OUTPUT);

  pinMode(PWM_D,   OUTPUT);

  pinMode(MtrD1,   OUTPUT);

  pinMode(MtrD2,   OUTPUT);

  Sentido(4);

  delay(2000);

}

Donde se definen cómo se comportan los pines -recuerdo que no es necesario declarar las salidas, ya lo son por defecto, pero si queda más fácil de leer el programa- con la llamada a la función "Sentido(4)" seteo los valores LOW en los cuatro pines que hacen al sentido de giro de los dos motores, apagándolos de esta forma, por último retraso en 2 segundos la continuación del programa, por comodidad nomás.

Seguidamente, el cuerpo principal del programa, o bucle infinito;

void loop() {

  D = Distancia();

  if (aux > 4) {

    (D>5)? Avanza(): Esquiva();

    analogWrite(PWM_I, 175); 

    analogWrite(PWM_D, 175); 

  } else aux++;

}

Esto se podría reducir a:

void loop() {

  (Distancia()>5)? Avanza(): Esquiva();

  analogWrite(PWM_I, 175); 

  analogWrite(PWM_D, 175); 

}

Y por supuesto funciona, evitando usar las dos variables declaradas. Simplemente basta con un condicional (usé la forma abreviada) donde si la distancia a un objeto es mayor a 5 centímetros, se avanza, de lo contrario se intenta esquivar el objeto. Y por supuesto, darle algún valor a los pulsos PWM que hacen a la velocidad de los motores, lógicamente hay una relación entre el tiempo de detención de los motores y la distancia recorrida por inercia, el límite de 5 cm podría ser conveniente subirlo si se desea usar los motores a máxima velocidad (255 en lugar de 175).

La razón por la que incrusté este código dentro de un condicional que no permita se ejecute movimiento alguno del vehículo hasta no se hayan hecho 5 disparos del sensor de ultrasonidos es para evitar mediciones erróneas.

A continuación, las diversas funciones declaradas en el programa, siendo la primera la que se encarga de medir la distancia al objeto:

unsigned long Distancia() {

  digitalWrite(DISPARO, LOW);

  delayMicroseconds(2);

  digitalWrite(DISPARO, HIGH);

  delayMicroseconds(10);

  digitalWrite(DISPARO, LOW);

  return int(0.017 * pulseIn(ECO, HIGH)); 

}

Funciona de la forma ya descrita en la entrada anterior, de hecho es el mismo código adaptado a una función, recuerdo que se trata de generar un pulso de 10 us en el pin que dispara al sensor para realizar una medición, para luego medir el ancho del pulso recibido en el pin ECO, y en función de este tiempo, calcular la distancia.

void Avanza() {

  Sentido(1);

}    

La función "Avanza()" no es más que una llamada a la función "Sentido(1)", que configura los pines que hacen a la dirección de giro de los motores en una dirección, la que personalmente adopté como frente del dispositivo, donde he puesto el sensor de ultrasonidos. No es necesario declararla, bien podría usarse Sentido(1) en su lugar, pero solo la hice con objeto de que el código sea lo más legible posible.

void Esquiva() {

  //Detiene

  Sentido(4); 

  delay(50);

  

  //Retrocede

  Sentido(0);

  delay(300);

  

  //Detiene

  Sentido(4); 

  delay(50);

  //Gira

  Sentido(random(2, 4));

  delay(random(250, 500));

  

  //Detiene

  Sentido(4); 

  delay(50);

}

En cambio, la función "Esquiva()" es más interesante de comentar, su algoritmo es el siguiente:

1) detiene los motores por 50 ms.

2) retrocede cambiando de sentido de giro ambos motores durante 300 ms.

3) detiene los motores por 50 ms.

4) invierte la dirección de giro de los motores al azar, haciendo que gire sobre su eje a izquierda o

    derecha, durante un tiempo -también al azar- de entre 250 y 499 ms.

5) detiene los motores por 50 ms.

No se puede hablar de verdadero azar ya que la función random() genera siempre los mismos valores, repitiéndolos cada vez que se resetea al ATMEGA, o dicho de otra forma, la secuencia para esquivar objetos es repetitiva cada vez que se arranca el juguete.

Se puede usar la instrucción randomSeed(millis()); en la función setup(), para cambiar la semilla de random según el mili segundo en que se activa al ATMEGA, dando entonces valores más aleatorios, y evitando así se repita la secuencia, pero para este ejemplo no es significativo.

Por último, la función "Sentido()", que hace a algunas posibles direcciones:

void Sentido(byte d) { 

  switch (d) {

    case 0: //Dirección 1

            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, LOW);  

            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, HIGH); 

            break;

    case 1: //Dirección 2

            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, HIGH); 

            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, LOW);  

            break;

    case 2: //Giro 1

            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, LOW);  

            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, LOW); 

            break;

    case 3: //Giro 2

            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, HIGH); 

            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, HIGH); 

            break;

    case 4: //Detiene (LOW)

            digitalWrite(MtrD1, LOW);  digitalWrite(MtrD2, LOW);  

            digitalWrite(MtrI1, LOW);  digitalWrite(MtrI2, LOW);  

            break;

    case 5: //Detiene (HIGH)

            digitalWrite(MtrD1, HIGH); digitalWrite(MtrD2, HIGH); 

            digitalWrite(MtrI1, HIGH); digitalWrite(MtrI2, HIGH); 

            break;

  }

}

No hay mucho que decir en cuanto a esta función, simplemente se encarga de setear los pines de dirección de los motores, en mi caso y según cómo tengo conectado los motores, cada caso se corresponde a la siguiente dirección:

caso 0: ambos motores giran en sentido anti horario.
caso 1: ambos motores giran en sentido horario.
caso 2: un motor en sentido anti horario, el otro en sentido horario.
caso 3: al revés que el caso 2.
caso 4: ambos motores detenidos, los cuatro pines en LOW (0 volts)
caso 5: ambos motores detenidos, los cuatro pines en HIGH (5 volts)

No preveo la necesidad de un motor girando y el otro detenido, ya sea en sentido anti horario u horario, tanto del lado izquierdo como del lado derecho, por lo menos, hasta ahora, no he necesitado de esas direcciones...

Y eso es todo, por el momento.

------------------------------------------------------------------------------------------------------------