Evitando obstáculos (3)

En esta nueva versión del programa anterior, utilizo los dos encoders ya instalados, que permiten contar pulsos desde las ruedas directamente, y así modificar la forma en que se esquivan los objetos, en lugar de hacerlo por tiempo, hacerlo a partir de cierta cantidad de pulsos.

El programa completo:

const int Ecd_0   =  2;

const int Ecd_1   =  3;

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;

unsigned long D = 0;

byte aux = 0;

void setup() {

  pinMode(Ecd_0,   INPUT );

  pinMode(Ecd_1,   INPUT );

  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);

  analogWrite(PWM_I, 175); 

  analogWrite(PWM_D, 175); 

  randomSeed(millis());

  Sentido(6);

  delay(1500);

}

void loop(){

  D = Distancia();

  if (aux > 4) {

    (D > 0 && D < 6)? Esquiva(): Avanza();

  } 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() {

  Sentido(6); //Detiene

  delay(50);

  Sentido(0); //Retrocede

  ContarPulsos(20);

  Sentido(6); //Detiene

  delay(50);

  

  Sentido(random(2, 6)); //Gira

  ContarPulsos(random(10, 25));

  Sentido(6); //Detiene

  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 ambos motores, uno al revés que el otro

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

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

            break;

    case 3: //Giro 2 ambos motores, al revés que Giro 1

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

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

            break;

    case 4: //Giro 3 un motor retrocede, el otro detenido

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

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

            break;

    case 5: //Giro 4 al revés que Giro 3

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

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

            break;

    case 6: //Detiene (LOW)

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

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

            break;

    case 7: //Detiene (HIGH)

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

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

            break;

  }

}

void ContarPulsos(byte p) {

  byte Pulsos_0 = 0;
  byte Pulsos_1 = 0;
  byte E_Encdr0 = LOW;
  byte E_Encdr1 = LOW;
  byte E_Encdr0Ant = LOW;
  byte E_Encdr1Ant = LOW;
  
  while (Pulsos_0 < p && Pulsos_1 < p) {
    E_Encdr0 = digitalRead(Ecd_0);
    E_Encdr1 = digitalRead(Ecd_1);
    if (E_Encdr0 != E_Encdr0Ant && E_Encdr0 == HIGH) Pulsos_0++;
    if (E_Encdr1 != E_Encdr1Ant && E_Encdr1 == HIGH) Pulsos_1++;
    E_Encdr0Ant = E_Encdr0;
    E_Encdr1Ant = E_Encdr1;
  }  
}

El programa inicia con las declaraciones de pines, donde incorporé los pines donde tengo conectado ambos encoders (pines 2 y 3), las variables globales son las ya comentadas en la entrada anterior, y en la función setup() agregué la instrucción randomSeed(millis()); a efectos de cambiar la semilla random para generar números al azar, así cómo trasladé las instrucciones analogWrite(PWM_I, 175); y analogWrite(PWM_D, 175); de modo de fijar las velocidades de los motores solo una vez.

La función loop() tiene una mínima modificación en el condicional que determina cuando debe esquivar un obstáculo, si la distancia al obstáculo se encuentra entre 1 cm y 5 cm entonces se llama a la función "Esquiva()", de lo contrario se llama a la función "Avanza()". Esto lo modifiqué en función del echo que si la distancia es mayor que el límite del sensor (entre 400 a 500 cm), éste devuelve 0 cm, y en espacios abiertos esto confunde al robot-juguete-auto, o cómo prefieran llamarlo.

La funciones "Distancia()" y "Avanza()" no tienen ninguna modificación con respecto a la entrada anterior, si la función "Esquiva()", en particular al momento de retroceder, donde retrocede 20 pulsos -20 es la cantidad de ranuras que tienen los discos asociados a las ruedas- o sea, retrocede una vuelta completa de ruedas. También modifiqué el giro, donde al azar se escogen entre cuatro posibles formas de girar, para lograr un efecto más vistoso simplemente. 

La función "Sentido()" -si bien ha sido modificada- no requiere de mayor comentarios, se explica por si sola.

Por último, la función "ContarPulsos(byte p)", la cual tiene la tarea de contar los pulsos leídos por los encoders en un bucle, comparándolos contra el valor ingresado en su parámetro de entrada "p". Y su pseudocódigo es el siguiente:

MIENTRAS (cualquiera de los dos contadores sean menor que p) HACER
  almacenar el estado actual del primer encoder
  almacenar el estado actual del segundo encoder
  SI (estado actual es distinto al estado anterior del primer encoder) ENTONCES
    aumentar su correspondiente contador en uno
  FIN SI
  SI (estado actual es distinto al estado anterior del segundo encoder) ENTONCES
    aumentar su correspondiente contador en uno

  FIN SI

  actualizar el estado anterior al estado actual del primer encoder

  actualizar el estado anterior al estado actual del segundo encoder

FIN MIENTRAS

Se cuentan pulsos cada vez que un encoder cambia su estado de LOW a HIGH, y basta que cualquiera de los dos contadores llegue a la cantidad deseada para salir del bucle, continuando con el programa.

De más está que diga que esta función puede ser mejorada, simplificada aprovechando las interrupciones, usar la librería NewPing.h, etcétera. El límite es la imaginación de quien quiera modificarla a su gusto.

Hasta aquí entonces, mi experiencia contando pulsitos...

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