Juntando todo (1)

Hay muchas razones por las que es conveniente transferir una parte de nuestro código a una o varias librerías, y  varios criterios a seguir en la forma de organizar dichas librerías, qué compartir y qué no, etcétera.

Tal vez, en los entornos de programación de Arduino, la razón más altruísta para compartir rutinas a través de librerías sea la de facilitarle la vida a otros programadores, compartiendo aquel código de uso general para -por ejemplo- el uso de un sensor o un actuador fácilmente, sin necesidad de que quien decida usar nuestra librería tenga que pasar por el derrotero de conocer a fondo el hardware que pretende usar, basta saber que es lo que hace dicho sensor u actuador para aplicarlo en nuestros proyectos, desentendiendonos del como lo hace. Por supuesto que siempre es bueno conocer el "cómo" de lo que sea que estemos usando, pero puede ser que se requiera de un nivel de conocimientos de electrónica que no todo programador posee, lo que en realidad termina en complicarle la vida.

Lo mío no es tan altruista, apenas intento solo simplificar mis futuros códigos, por lo que he puesto aquellas rutinas de uso repetitivo en mis propios proyectos en una librería de nombre muy poco original (MiselaneasRobot01) con objeto de simplificarme la vida a mi mismo...

Cómo sea, en el siguiente link se explica muy bien cuales son los pasos a seguir para crear una librería para Arduino, y existen muchos otros tutoriales disponibles en la red:

http://playground.arduino.cc/ArduinoNotebookTraduccion/Appendix2

Siguiendo entonces el tutorial recomendado en la página oficial de Arduno, construí los siguientes archivos: MiselaneasRobot01.h, MiselaneasRobot01.cpp y keywords.txt siendo el primero (.h) el encabezado de la librería, el segundo (.cpp) el código de la librería y el tercero (.txt) el de enlace de las nuevas funciones al entorno arduino, para que éstas se vean resaltadas en color al usarlas en mis programas. También los programas ejemplos de uso, pero los comentaré en la siguiente entrada.

El código del archivo de encabezado es el siguiente:

#ifndef MiselaneasRobot01_h
    #define MiselaneasRobot01_h
   
    //Constantes literales
    #define Retroceder  0
    #define Avanzar     1
    #define Giro_1      2
    #define Giro_2      3
    #define Giro_3      4
    #define Giro_4      5
    #define DetenerLOW  6
    #define DetenerHIGH 7
   
    class MiselaneasRobot01 
    {
      public:
        MiselaneasRobot01(int pinDISPR, int pinECO);
       
        MiselaneasRobot01(int pinMtrD1, int pinMtrD2, int pinPWM_D,
                          int pinMtrI1, int pinMtrI2, int pinPWM_I);
                          
        MiselaneasRobot01(int pinENCD0, int pinENCD1,
                          float LimCorrecn);
       
        unsigned long Distancia_cm();
        void Direccion(int direccion);
        void ContarPulsos(int p);
        void FijarVelocidades(int Vel0, int Vel1);
        void CorregirVelocidades(int &Vel0, int &Vel1);
        void DetenerInterrupciones();                                 
        void ActivarInterrupciones();
       
      private:
        int _pinDISPR;
        int _pinECO;
        int _pinMtrD1;
        int _pinMtrD2;
        int _pinPWM_D;
        int _pinMtrI1;
        int _pinMtrI2;
        int _pinPWM_I;
        int _pinENCD0;
        int _pinENCD1;
        float _LimCorrecn;
    };

#endif

Al inicio del archivo de encabezados, definí las constantes literales que estarán disponibles cómo las ocho posibles direcciones a tomar por los dos motores del robot, luego, en la parte pública de la clase, declaro tres objetos, el primero pensado en el uso de forma independiente del sensor de ultrasonidos, el segundo para el tratamiento de los motores y el tercero para encoders, continúo con la declaración (pública) de las funciones disponibles. Por último, en la zona de declaraciones privadas, las variables internas de trabajo de la librería, que se corresponden en cantidad y tipo con todos los parámetros de los tres objetos.

Este encabezado me permite usar los objetos de varias formas, es válido desde el entorno de Arduino declarar los objetos:

#include <MiselaneasRobot01.h>

MiselaneasRobot01 Ultrasonido(4, 5);
MiselaneasRobot01 Motores(12, 13, 11, 7, 8, 6);
MiselaneasRobot01 Encoders(2, 3, 0.95);

Y, al momento de usar las diversas funciones, será perfectamente posible hacer lo siguiente:

  D = Ultrasonido.Distancia_cm();

  if (D > 10 && D < 100) {
    Motores.Direccion(Avanzar);
    Encoders.CorregirVelocidades(Vel_0, Vel_1);
  ...

Pero, dado que todo es parte de la misma clase; QUALQUIER FUNCIÓN ESTÁ DISPONIBLE EN CUALQUIER OBJETO, o sea, con esa misma declaración de objetos, sería válido usar:

D = Ultrasonido.Distancia_cm();

  if (D > 10 && D < 100) {
    Ultrasonido.Direccion(Avanzar);
    Ultrasonido.CorregirVelocidades(Vel_0, Vel_1);
  ...

Lo que podría ser un poco confuso, o no, dependiendo ya de las declaraciones de los objetos y del programador usuario de la librería en sí; por ejemplo si uno de los tres objetos se llamara "robot" esto podría ser escrito de la siguiente forma:

D = robot.Distancia_cm();

  if (D > 10 && D < 100) {
    robot.Direccion(Avanzar);
    robot.CorregirVelocidades(Vel_0, Vel_1);
  ...

Que -por lo menos a mi juicio- hasta quedaría mucho más genérico y fácil de entender al momento de interpretar un programa realizado para el robot en cuestión.

En fin, queda entonces en manos de quien quiera usar esta librería la forma de cómo usarla, o corregirla y mejorarla, o agregarle más funciones... como siempre, el límite es nuestra imaginación.

La implementación del código es el siguiente:

#include "WProgram.h"
#include "MiselaneasRobot01.h"

volatile int _Pulsos0;
volatile int _Pulsos1;
unsigned long _msTranscurridos;

MiselaneasRobot01::MiselaneasRobot01(int pinDISPR, int pinECO) {
  pinMode(pinDISPR, OUTPUT);
  pinMode(pinECO, INPUT);
 
  _pinDISPR = pinDISPR;
  _pinECO   = pinECO;
}

MiselaneasRobot01::MiselaneasRobot01(int pinMtrD1, int pinMtrD2, int pinPWM_D,
                                     int pinMtrI1, int pinMtrI2, int pinPWM_I) {
  pinMode(pinMtrD1, OUTPUT);
  pinMode(pinMtrD2, OUTPUT);
  pinMode(pinPWM_D, OUTPUT);
  pinMode(pinMtrI1, OUTPUT);
  pinMode(pinMtrI2, OUTPUT);
  pinMode(pinPWM_I, OUTPUT);

  _pinMtrD1 = pinMtrD1;
  _pinMtrD2 = pinMtrD2;
  _pinPWM_D = pinPWM_D;
  _pinMtrI1 = pinMtrI1;
  _pinMtrI2 = pinMtrI2;
  _pinPWM_I = pinPWM_I;
}

MiselaneasRobot01::MiselaneasRobot01(int pinENCD0, int pinENCD1, 
                                     float LimCorrecn) {
  pinMode(pinENCD0, INPUT);
  pinMode(pinENCD1, INPUT);

  _pinENCD0   = pinENCD0;
  _pinENCD1   = pinENCD1;
  _LimCorrecn = LimCorrecn;
}
                                          
unsigned long MiselaneasRobot01::Distancia_cm() {
  digitalWrite(_pinDISPR, LOW ); delayMicroseconds(2);
  digitalWrite(_pinDISPR, HIGH); delayMicroseconds(10);
  digitalWrite(_pinDISPR, LOW );
  return int(0.017 * pulseIn(_pinECO, HIGH));
}

void MiselaneasRobot01::Direccion(int direccion) {
  switch (direccion) {
    case Retroceder:
            digitalWrite(_pinMtrD1, HIGH); digitalWrite(_pinMtrD2, LOW ); 
            digitalWrite(_pinMtrI1, LOW ); digitalWrite(_pinMtrI2, HIGH);
            break;
    case Avanzar:
            digitalWrite(_pinMtrD1, LOW ); digitalWrite(_pinMtrD2, HIGH);
            digitalWrite(_pinMtrI1, HIGH); digitalWrite(_pinMtrI2, LOW ); 
            break;
    case Giro_1: //ambos motores, uno al revés que el otro
            digitalWrite(_pinMtrD1, HIGH); digitalWrite(_pinMtrD2, LOW ); 
            digitalWrite(_pinMtrI1, HIGH); digitalWrite(_pinMtrI2, LOW );
            break;
    case Giro_2: //ambos motores, al revés que Giro 1
            digitalWrite(_pinMtrD1, LOW ); digitalWrite(_pinMtrD2, HIGH);
            digitalWrite(_pinMtrI1, LOW ); digitalWrite(_pinMtrI2, HIGH);
            break;
    case Giro_3: //un motor retrocede, el otro detenido
            digitalWrite(_pinMtrD1, HIGH); digitalWrite(_pinMtrD2, LOW ); 
            digitalWrite(_pinMtrI1, LOW ); digitalWrite(_pinMtrI2, LOW );
            break;
    case Giro_4: //al revés que Giro 3
            digitalWrite(_pinMtrD1, LOW ); digitalWrite(_pinMtrD2, LOW ); 
            digitalWrite(_pinMtrI1, LOW ); digitalWrite(_pinMtrI2, HIGH);
            break;
    case DetenerLOW:
            digitalWrite(_pinMtrD1, LOW ); digitalWrite(_pinMtrD2, LOW ); 
            digitalWrite(_pinMtrI1, LOW ); digitalWrite(_pinMtrI2, LOW ); 
            break;
    case DetenerHIGH:
            digitalWrite(_pinMtrD1, HIGH); digitalWrite(_pinMtrD2, HIGH);
            digitalWrite(_pinMtrI1, HIGH); digitalWrite(_pinMtrI2, HIGH);
            break;
  }
}

void Cuenta(int &ecd, int &pin, int &ecdant, int &pls) {
  ecd = digitalRead(pin);
  if (ecd != ecdant && ecd == HIGH) pls++;
  ecdant = ecd;
}

void MiselaneasRobot01::ContarPulsos(int p) {
  int Pulsos_0    = 0;
  int Pulsos_1    = 0;
  int E_Encdr0    = LOW;
  int E_Encdr1    = LOW;
  int E_Encdr0Ant = LOW;
  int E_Encdr1Ant = LOW;
 
  while (Pulsos_0 < p && Pulsos_1 < p) {
    Cuenta(E_Encdr0, _pinENCD0, E_Encdr0Ant, Pulsos_0);
    Cuenta(E_Encdr1, _pinENCD1, E_Encdr1Ant, Pulsos_1);
  } 
}

int AsegurarRango(int &V) {
  if (V < 0) V = 0; else if(V > 255) V = 255;
  return V;    
}
    
void MiselaneasRobot01::FijarVelocidades(int Vel0, int Vel1) {
  analogWrite(_pinPWM_I, AsegurarRango(Vel0));
  analogWrite(_pinPWM_D, AsegurarRango(Vel1));
}

void Correccion(volatile int &Pa, volatile int &Pb,
                         int &VS, int &Vx, float &LC) {
  if (Pa > Pb) VS = VS - (Pa - Pb);
  if (VS < (Vx * LC)) VS = Vx * LC;
}

void MiselaneasRobot01::CorregirVelocidades(int &Vel0, int &Vel1) {
  if (_LimCorrecn > 0.0) {
    if (millis() - _msTranscurridos >= 500) {
      DetenerInterrupciones(); 
      _msTranscurridos = millis();
      Correccion(_Pulsos0, _Pulsos1, Vel0, Vel1, _LimCorrecn);
      Correccion(_Pulsos1, _Pulsos0, Vel1, Vel0, _LimCorrecn);
      ActivarInterrupciones();
    }
  }
}

void MiselaneasRobot01::DetenerInterrupciones() {
  detachInterrupt(0);
  detachInterrupt(1);
}

void ContarPulsos0() { _Pulsos0++; }
void ContarPulsos1() { _Pulsos1++; }

void MiselaneasRobot01::ActivarInterrupciones() {
  _Pulsos0 = 0;
  _Pulsos1 = 0;
  attachInterrupt(0, ContarPulsos0, CHANGE);
  attachInterrupt(1, ContarPulsos1, CHANGE);
}

En principio, se trata de una transcripción de las funciones ya usadas y comentadas en entradas anteriores, adaptadas al formato de una librería como bien se explica en la página oficial de Arduino (ver link de más arriba). También traté de optimizar aquellas funciones que en mis originales tenían algún código repetido.

En el código de los tres objetos simplemente se configuran los pines digitales según correspondan en entradas o salidas y se transfieren el contenido de los parámetros de cada objeto a las correspondientes variables privadas de la librería, declaradas en el archivo de encabezados. E inmediatamente después ya está el desarrollo de cada función:

• Distancia_cm: Función pública, usa la placa de ultrasonidos, mide distancias en centímetros.
• Direccion: Función pública, setea los pines que hacen al sentido de giro de los motores.
• Cuenta: Función interna, usada por la función ContarPulsos.
• ContarPulsos: Función pública, usa a los dos encoders para contar pulsos directamente desde los motores.
• AsegurarRango: Función interna, usada por la función FijarVelocidades.
• FijarVelocidades: Función pública, setea el valor de los PWM de cada motor.
• Correccion: Función interna, usada por la función CorregirVelocidades.
• CorregirVelocidades: Función pública, cuando se alcanza un determinado tiempo, se aplica una corrección en las velocidades de ambos motores, se utilizan las interrupciones propias del ATMEGA para este fin.
• DetenerInterrupciones: Función pública, permite detener las interrupciones del ATMEGA a voluntad del usuario.
• ActivarInterrupciones: Función pública, permite activar las interrupciones del ATMEGA a voluntad del usuario. 
• ContarPulsos0 y ContarPulsos1: Funciones internas, actualizan contadores relacionados con las interrupciones.

Por último, el archivo keywords.txt, que no merece más comentario del que se encuentra en el sitio oficial. Yo simplemente copié el contenido de un archivo disponible en otra de las librerías del entorno Arduino, para luego modificar las "palabras reservadas" que me interesan se "coloreen" al usar la librería en algún programita.

###########################################
# Syntax Coloring Map for MiselaneasRobot01
###########################################

###########################################
# Datatypes (KEYWORD1)
###########################################
MiselaneasRobot01    KEYWORD1

###########################################
# Methods and Functions (KEYWORD2)
###########################################
Distancia_cm    KEYWORD2
Direccion    KEYWORD2
ContarPulsos    KEYWORD2
FijarVelocidades    KEYWORD2
CorregirVelocidades    KEYWORD2
DetenerInterrupciones    KEYWORD2
ActivarInterrupciones    KEYWORD2

###########################################
# Instances (KEYWORD2)
###########################################

###########################################
# Constants (LITERAL1)
###########################################
Retroceder    LITERAL1
Avanzar    LITERAL1
Giro_1    LITERAL1
Giro_2    LITERAL1
Giro_3    LITERAL1
Giro_4    LITERAL1
DetenerLOW    LITERAL1
DetenerHIGH    LITERAL1

Nada más por el momento, gracias por tu tiempo leyendo esto y tu buena predisposición a tratar de entenderlo.

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

Buscando Obstáculos (2)

Se trata de hacer exactamente lo mismo que en la entrada anterior, pero evitando la librería "NewPing.h".

El programa es prácticamente el mismo, simplemente en lugar de usar el comando "Radar.ping_cm()" uso mi función para medir distancias, ya explicada, solo la renombré a "Distancia_cm()" que, creo, es un nombre un poquito más significativo.

Otra modificación (no necesaria en realidad) se encuentra dentro de la función "SonarAlarma()", donde en lugar de llamar a la función "Inicializar()" utilizo el comando "asm("jmp 0x0000")".

Este comando lo que hace es -desde asembler- saltar incondicionalmente a la primera línea del programa, es, por definirlo de algún modo, una especie de reset al programa en si, no al hardware. 

No existe ningún comando o directiva (al menos que yo conozca) que permita realizar un reset del ATMEGA, y, si bien podemos imaginarnos alguna simple electrónica para disparar el pin de reset del hardware, en la práctica no le encuentro necesidad, por lo menos en lo aquí descrito, tal vez algún día si.

A continuación el programa completo:

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

volatile byte Pulsos_0, Pulsos_1;

unsigned long msTranscurridos;

int Vel_0, Vel_1;

byte aux, busquedas, girar;

void ContarPulsos0() { Pulsos_0++; }

void ContarPulsos1() { Pulsos_1++; }

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

  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 {

    Distancia_cm();

    aux++;

  }

}

void BuscarObjetivo() {

  Vel_0 = V_Crucero * LimCorrecn;

  Vel_1 = V_Crucero * LimCorrecn;

  

  if (Distancia_cm()!=0 && Distancia_cm()<MaxDisObjt) Avanzar();

  else

    if (busquedas < 14) {

      Sentido(girar);

      ContarPulsos(3);

      Sentido(DetineLOW);

      busquedas++;

      delay(250);

    } else busquedas = 0;

}

void Avanzar() { 

  if (Distancia_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 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);

}

void SonarAlarma() {

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

    Sentido(DetineHIGH);

    delay(500);

    Sentido(DetineLOW);

    delay(250);

  }

  asm("jmp 0x0000"); //Inicializar(); 

}

unsigned long Distancia_cm() {

  digitalWrite(DISPARO, LOW);

  delayMicroseconds(2);

  digitalWrite(DISPARO, HIGH);

  delayMicroseconds(10);

  digitalWrite(DISPARO, LOW);

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

}

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

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.

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