Buscando Obstáculos (1)

PSEUDO SEGUIDOR DE OBJETOS

Continuando con el sensor de ultrasonidos, me propuse hacer que el robot cumpla con una nueva función, la de buscar un objeto para ir a su encuentro.

Antes de explicar el programa, debo comentar un pequeño problemita con el hardware que me "volvió loco" (más de lo normal) durante un tiempo considerable, se trata del ángulo de de medición del sensor, que es de 15° en todas las direcciones.

Por la forma de propagación de la señal ultrasónica en la distancia y la altura del sensor con respecto al piso, al poner un límite mayor de 50 cm como rango de búsqueda, el robot no se comportaba de acuerdo a lo esperado, estimo que esto se debe a que la señal rebota en el piso y se desvía o regresa al sensor en forma alterada, dando como resultado falsas mediciones.

Por lo pronto, solucioné el problema inclinando algunos grados al sensor con respecto a la perpendicular del soporte donde lo instalé (para que apunte un poco hacia arriba), nada muy original, pero si lo suficiente para lograr que se comporte dentro de lo esperado en un rango de hasta 150 cm.

Aclarado este detalle, vamos por el programa:

#include <NewPing.h>

#define Disparo    4   

#define Eco        5   

#define MaxDistTot 300 

#define MaxDisObjt 150

#define MinDisObjt 10

#define Interrupn0 0

#define Interrupn1 1

#define LimCorrecn 0.75

#define T_Interrup 500

#define V_Crucero  200

#define Retrocede  0

#define Avanza     1

#define Giro_1     2

#define Giro_2     3

#define Giro_3     4

#define Giro_4     5

#define DetineLOW  6

#define DetineHIGH 7

const int Ecd_0 =  2;

const int Ecd_1 =  3;

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;

volatile byte Pulsos_0, Pulsos_1;

unsigned long msTranscurridos;

int Vel_0, Vel_1;

byte aux, busquedas, girar;

NewPing Radar(Disparo, Eco, MaxDistTot); 

void ContarPulsos0() { Pulsos_0++; }

void ContarPulsos1() { Pulsos_1++; }

void setup() {

  pinMode(Ecd_0, INPUT );

  pinMode(Ecd_1, INPUT );

  pinMode(PWM_I, OUTPUT);

  pinMode(MtrI1, OUTPUT);

  pinMode(MtrI2, OUTPUT);

  pinMode(PWM_D, OUTPUT);

  pinMode(MtrD1, OUTPUT);

  pinMode(MtrD2, OUTPUT);

  randomSeed(millis());

  Inicializar();

}

void Inicializar() {

  Sentido(DetineLOW);

  Pulsos_0        = 0;

  Pulsos_1        = 0;

  msTranscurridos = 0;

  Vel_0           = 0;

  Vel_1           = 0;

  aux             = 0;

  busquedas       = 0;

  girar = random(Giro_1, DetineLOW);

  delay(5000);

}

void loop() {

  if (aux > 4)

    BuscarObjetivo();

  else {

    Radar.ping_cm();

    aux++;

  }

}

void BuscarObjetivo() {

  Vel_0 = V_Crucero * LimCorrecn;

  Vel_1 = V_Crucero * LimCorrecn;

  

  if(Radar.ping_cm()!=0 && Radar.ping_cm()<=MaxDisObjt) Avanzar();

  else 

    if (busquedas < 14) {

      Sentido(girar);

      ContarPulsos(3);

      Sentido(DetineLOW);

      busquedas++;

      delay(250);

    } else Inicializar();

}

  

void Avanzar() { 

  if (Radar.ping_cm() < MinDisObjt) SonarAlarma();

  else {

    ActivarInterrupciones();

    Vel_0 = V_Crucero;

    Vel_1 = V_Crucero;

    if (millis() - msTranscurridos >= T_Interrup) {

      DetenerInterrupciones();

      msTranscurridos = millis();

      if(Pulsos_0>Pulsos_1) Vel_0 = Vel_0 - ((Pulsos_0 - Pulsos_1));

      if(Pulsos_1>Pulsos_0) Vel_1 = Vel_1 - ((Pulsos_1 - Pulsos_0));

      if(Vel_0<(Vel_1 * LimCorrecn))Vel_0 = Vel_1 * LimCorrecn;

      if(Vel_1<(Vel_0 * LimCorrecn))Vel_1 = Vel_0 * LimCorrecn;

      Pulsos_0 = 0;

      Pulsos_1 = 0;

    } 

    Sentido(Avanza);

  }

}    

void SonarAlarma() {

  for(int i=0; i<5; i++) {

    Sentido(DetineHIGH);

    delay(500);

    Sentido(DetineLOW);

    delay(250);

  }

 Inicializar(); 

}

void Sentido(byte d) { 

  switch (d) {

    case Retrocede: 

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

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

            break;

    case Avanza:

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

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

            break;

    case 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 Giro_2: //ambos motores, al revés que Giro 1

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

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

            break;

    case Giro_3: //un motor retrocede, el otro detenido

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

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

            break;

    case Giro_4: //al revés que Giro 3

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

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

            break;

    case DetineLOW: 

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

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

            break;

    case DetineHIGH:

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

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

            break;

  }

  analogWrite(PWM_I, Vel_0); 

  analogWrite(PWM_D, Vel_1); 

}

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;

  }  

}

void DetenerInterrupciones() {

  detachInterrupt(Interrupn0);

  detachInterrupt(Interrupn1);

}

void ActivarInterrupciones() {

  attachInterrupt(Interrupn0, ContarPulsos0, CHANGE);

  attachInterrupt(Interrupn1, ContarPulsos1, CHANGE);

}

En este programa usé la librería NewPing ya comentada en una entrada anterior para luego crear al objeto "Radar".

A continuación las constantes definidas, que representan lo siguiente:

• "Disparo" y "Eco" son los pines usados por el sensor, 
• "MaxDistTot" representa la máxima distancia de trabajo del objeto Radar, 
• "MaxDisObjt" el el límite hasta donde se interpreta que existe un objeto, y 
• "MinDisObjt" se ocupa de fijar el límite hasta donde se acercará el robot al objeto detectado.

• "Interrupn0" representa la interrupción 0 (pin 2 del metaboard).
• "Interrupn1" representa la interrupción 1 (pin 3 del metaboard).
• "LimCorrecn" se trata de un valor de límite de corrección usada para igualar velocidades.
• "T_Interrup" es el tiempo destinado a contar pulsos para verificar diferencias entre motores.
• "V_Crucero" velocidad crucero, velocidad constante para los motores.
• "Retrocede", "Avanza", "Giro_1", "Giro_2", "Giro_3" y "Giro_4" hacen a las posibles direcciones del robot, mientras que "DetineLOW" y "DetineHIGH" además de detener al robot, las uso para generar un parpadeo del led conectado al pin 13, a modo de alarma.

• "Ecd_0" y "Ecd_1" representan los pines donde se conectan los encoders. 
• "PWM_I", "MtrI1" y "MtrI2" los pines para controlar al motor izquierdo.
• "PWM_D", "MtrD1" y "MtrD2" los pines para controlar al motor derecho.

Las variables globales "Pulsos_0" y "Pulsos_1" almacenan el conteo de pulsos leídos desde los encoders, "msTranscurridos" almacena los mili segundos que transcurren para disparar las correcciones de velocidades, "Vel_0" y "Vel_1" guardan la velocidad final a aplicar a cada motor y "aux", "busquedas" y "girar" son en realidad tres variables auxiliares.

A continuación se declara el objeto "Radar" desde la librería NewPing y las funciones que se ejecutan en cada interrupción.

setup() define el comportamiento de los pines del metaboard, cambia la semilla para generar números al azar e invoca a la función Inicializar().

Inicializar() tiene por objeto inicializar al robot en sí, la función es independiente de setup() pues se reinicializa el robot según distintas situaciones, ya sea al terminar de hacer una búsqueda y no encontrar objeto o al alcanzar un objeto detectado.

loop() se encarga de realizar 5 disparos del sensor a efectos de eliminar lecturas erróneas (cuenta usando variable aux); cumplidos los 5 disparos invoca permanentemente a la función BuscarObjetivo().

BuscarObjetivo() reduce la velocidad de los motores en el mismo porcentaje del límite de corrección, y consulta al Radar para detectar si existe un objeto en su campo de búsqueda, de existir llama a la función Avanzar(), de no existir gira en alguno de las posibles direcciones previstas por tres pulsos contados desde los motores y detiene al robot por un pequeño tiempo, esto lo repite catorce veces, que es aproximadamente un giro completo del robot cuando gira sobre su eje, de no encontrar nada y terminar su ciclo de búsqueda, inicializa al robot.

Avanzar() tiene por objeto la de desplazar al robot hasta el objetivo, al alcanzarlo dispara una alarma de aviso. Mientras se desplaza realiza las pertinentes correcciones de las velocidades, según lo ya comentado en entradas anteriores.

SonarAlarma() en esta versión simplemente aprovecha al led conectado al pin trece haciendolo parpadear, pero bien podría disparar un buzzer si se implementa la electrónica correspondiente.

Las funciones Sentido(), ContarPulsos(), DetenerInterrupciones() y ActivarInterrupciones() ya se comentaron en entradas anteriores.

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