Juntando todo (2)

Continuando con el uso de la librería desarrollada para el robot, se recomienda la realización de pequeños programas de muestra del uso de sus funciones, y, si bien la librería es exclusiva para una aplicación en concreto y poca utilidad tendría en otros desarrollos, no está de más seguir con los consejos de la página oficial de Arduino en lo que a librerías concierne, aunque más no sea para recordar rápidamente que hace cada función.

Los siguientes cuatro programas están destinados a la verificación de las funciones de la librería.

En el código siguiente se crea el objeto Motores, pasando cómo parámetros los cuatro pines de conexión hacia el C.I. L298N, el objetivo del programa es el de mover a velocidad constante el robot en las ocho direcciones previstas durante un determinado tiempo, al finalizar el ciclo enciende y apaga el led del pin 13 cuatro veces, antes de repetir nuevamente el ciclo:

#include <MiselaneasRobot01.h>

MiselaneasRobot01 Motores(12, 13, 11, 7, 8, 6);

void setup() {
  Motores.Direccion(DetenerLOW);
  Motores.FijarVelocidades(200, 200);
}

void loop() {
  delay(2500);
  Motores.Direccion(Avanzar);    delay(1000); 

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Retroceder); delay(1000); 

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Giro_1);     delay(500);

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Giro_2);     delay(500);

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Giro_3);     delay(750);  

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Giro_4);     delay(750);

  Motores.Direccion(DetenerLOW);

  for (int i = 0; i < 5; i++) {
    Motores.Direccion(DetenerHIGH); delay(250);
    Motores.Direccion(DetenerLOW);  delay(250);
  }
}

El siguiente código hace lo mismo que el anterior, con la diferencia de que en lugar de probar las posibles direcciones durante un determinado tiempo, las verifica durante cierta cantidad de pulsos recibidos desde los encoders. Se crea el objeto Encoders, pasando cómo parámetros los pines donde se recibe la información, y un valor de 0.0 que anula la posibilidad de corregir velocidades:

#include <MiselaneasRobot01.h>

MiselaneasRobot01 Motores(12, 13, 11, 7, 8, 6);
MiselaneasRobot01 Encoders(2, 3, 0.0);

void setup() {
  Motores.Direccion(DetenerLOW);
  Motores.FijarVelocidades(200, 200);
}

void loop() {
  delay(2500);
  Motores.Direccion(Avanzar);    Encoders.ContarPulsos(100); 

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Retroceder); Encoders.ContarPulsos(100);

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Giro_1);     Encoders.ContarPulsos(20);

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Giro_2);     Encoders.ContarPulsos(20);

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Giro_3);     Encoders.ContarPulsos(40);

  Motores.Direccion(DetenerLOW);
  Motores.Direccion(Giro_4);     Encoders.ContarPulsos(40);

  Motores.Direccion(DetenerLOW);

  for (int i = 0; i < 5; i++) {
    Motores.Direccion(DetenerHIGH);
    delay(250);
    Motores.Direccion(DetenerLOW);
    delay(250);
  }
  delay(5000);
}

El objetivo del siguiente programa es avanzar y retroceder en línea recta. Se aplica corrección de velocidades hasta 0.95 del valor de PWM de cada motor:

#include <MiselaneasRobot01.h>

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

int Vel_0, Vel_1;
byte aux = 0;

void setup() { }

void loop() {
  Vel_0 = 200;
  Vel_1 = 200;
  Motores.Direccion(DetenerLOW);
  delay(2500);

  Motores.CorregirVelocidades(Vel_0, Vel_1);
  Motores.FijarVelocidades(Vel_0, Vel_1);

  if (aux == LOW) {
    aux = HIGH;
    Motores.Direccion(Avanzar);
    Encoders.ContarPulsos(150);
  } else { 
    aux = LOW;
    Motores.Direccion(Retroceder);
    Encoders.ContarPulsos(150);
  }
}

Por último, un pequeño programa que sigue a un objeto que se encuentre delante del robot hasta una distancia de 100 cm. Se utiliza a la placa de ultrasonidos:

#include <MiselaneasRobot01.h>

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

int Vel_0, Vel_1;
unsigned long D;

void setup() {
  Motores.Direccion(DetenerLOW);
  delay(2500);
}

void loop() {
  Vel_0 = 255;
  Vel_1 = 255;
  D = Ultrasonido.Distancia_cm();
 
  if (D > 10 && D < 100) {
    Motores.Direccion(Avanzar);
    Encoders.CorregirVelocidades(Vel_0, Vel_1);
    Motores.FijarVelocidades(Vel_0, Vel_1);
  } else { 
    Motores.Direccion(DetenerLOW);
    Encoders.DetenerInterrupciones();
  }
}

Verificado entonces la funcionalidad de la librería, queda disponible para ser usada ya en un proyecto final.

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

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

}

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