Robot esquiva obstáculos con Arduino

De castillowiki
Ir a la navegación Ir a la búsqueda

En este artículo vamos a construir un robot muy específico con Arduino, un robot que esquiva obstáculos y consigue salir de un laberinto.

En Internet encontrarás muchos tutoriales, la mayoría utiliza diferentes drivers para los motores. Muchos tienen un sólo sensor ultrasónico que se mueve con una torreta hecha con un servo. Elijas el que elijas, fíjate en que el código es distinto. El robot que describe este artículo tiene el Arduino Motor Shield Rev3 y 3 sensores ultrasónicos. Para moverse, utilitza dos ruedas motrices y una pivotante que, en nuestro caso, está situada en la parte delantera.

Construcción del Robot

Materiales

El chasis que verás en las fotos es específico, creado a medida por la empresa What's Next? para el proyecto [Robots Boost Skills]. El resto de componentes son genéricos y se pueden comprar Arduinos oficiales, What's Next Yellow o cualquier clon compatible.

Esta es la lista de materiales:

  • Chasis que permita 2 ruedas con motor DC analógicos y una rueda delantera.
  • 2 Motores DC analógicos con reducción y ruedas.
  • Arduino Uno o equivalente.
  • 3 Sensores ultrasónicos HR-SC04 o SRF05
  • Arduino Motor Shield o alguno que tenga el mismo Chip L298
  • Baterías, entre 9V y 12V

Fotos

Construcción del Chasis

En el caso del robot del ejemplo, el chasis tiene todos los elementos necesarios. Si lo tienes que construir, aquí tienes algunos consejos:

  • Se recomienda cuidar el centro de gravedad de robot para que no plante rueda y tenga la adherencia necesaria. Por ejemplo, las baterías deberían estar entre las ruedas motrices y la rueda delantera.
  • Los sensores deberían estar situados de forma que se cubra el mismo ángulo por los dos lados. En nuestro caso, al haber 3 sensores, se han puesto uno en el centro hacia adelante y los otros en un ángulo de 45º. Sin embargo, nosostros podemos girar el sensor al ángulo que deseemos y para seguir laberintos es mejor ponerlo en ángulo de 90º.
  • Hay que dejar espacio para los cables y para poder modificar las conexiones sin necesidad de desmontar todo el robot.
  • Hay muchos cables por recovecos muy pequeños y sometidos a tensiones, no es extraño que alguno se suelte, por lo que recomendamos usar alguna fijación. En nuestro caso una gota de cola termofusible.
  • Se recomienda probar los motores y su dirección en función de la polaridad para no tener que modificar el código.

Cableado

Hay que tenen en cuenta que el Arduino Motor Shield utiliza algunos pines para su funcionamiento:

Function 	A 	 B
Direction 	D12 	D13
PWM 	        D3 	D11
Brake 	        D9 	D8
Curr. Sensor 	A0 	A1

Por tanto, no podemos utilizar esos pines. Cada sensor necesita 2 pines digitales para el echo i el trig. Así que necesitamos ocupar 6 pines digitales, más los 6 para los motores. Para los sensores utilitzaremos los pines 2,4,5,6,7,10.

Este es el esquema de cables del robot:

Robot bb.png

Alimentaremos tanto al Arduino como al Shield por la entrada del shield. Este alimenta al arduino por el pin VIN. Como el voltaje va a ser moderado, no es necesario aislar el Arduino del shield. Si los motores necesitaran más de 9V hay que cortar el pin VIN y alimentar por separado el Arduino.

Programación del Robot

Como este artículo pretende ser didàctico, no vamos a poner la solución final diréctamente (tampoco hay una solución final perfecta). Iremos poniendo versiones distintas en las que se explican las sucesivas mejoras.

Leyendo de los sensores

El siguiente ejemplo lee la distancia de los sensores y la imprime por el puerto serie:

//Declaración de las variables de los pines:
int trigiz = 7;
int echoiz = 10;
int trigcen = 5;
int echocen = 6;
int trigder = 2;
int echoder = 4;
int distiz, distcen, distder; //Las variables de las distancias

void setup()
{
 pinMode(trigiz , OUTPUT);  // Los trig son de salida
 pinMode(trigcen , OUTPUT);
 pinMode(trigder , OUTPUT);
 pinMode(echoiz , INPUT);   // Los echo son de entrada
 pinMode(echocen , INPUT);
 pinMode(echoder , INPUT);
 Serial.begin(9600);
}
int ping(int trig, int echo){
  digitalWrite(trig,LOW);  
  delayMicroseconds(2);    // Deja en LOW 2 microsegundos para "resetar" el sensor
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);   // Ponemos el trig a HIGH durante 10 microsegundos, el sensor enviará luego 8 pulsos
  digitalWrite(trig,LOW);
  float duration = pulseIn(echo,HIGH); // Capta el tiempo que pasa hasta que retornan los pulsos.
  int distance = duration * 0.034 / 2; // Cálculo de la distancia en cm
  return(distance);
}

void loop()
{
  distiz = ping(trigiz,echoiz);   // Llama a la función ping para el sensor izquierdo.
  Serial.print("Izquierda: "); Serial.print(distiz);
  delay(50);
  distcen = ping(trigcen,echocen);
  Serial.print(" Centro: "); Serial.print(distcen);
  delay(50);
  distder = ping(trigder,echoder);
  Serial.print(" Derecha: "); Serial.println(distder);
  delay(50);
}
El sensor tiene aproximadamente 55º y es más efectivo en la zona sombreada.

Como se ve, necesita enviar un trig de al menos 10 microsegundos, cuando acaba el trig, el sensor enviará 8 pulsos a 40KHz y escuchar hasta que vuelve. Esta escucha se hace poniendo el echo a 1, cuando el sonido retorna se pone a 0 y el resultado es el tiempo que ha tardado de 1 a 0 (pulseIn). Después calcula la distancia teniendo en cuenta la velocidad del sonido.

Hc-sr04-timing-chart.png

El problema del ejemplo anterior es que al echo le pueden afectar muchos factores físicos del mundo real y puede que la medición no sea ajustada. Por eso, es mejor hacer varias mediciones, descartar valores extremos y calcular la media. Todo esto tiene un coste computacional que puede que nos afecte al rendimiento, por lo que puede que calcular la media o no hacer nada sea suficiente. Si queremos saber más, podemos buscar por filter o smoothing de los sensores en un buscador.
Multiplicar por 0.034 y dividir entre 2 puede ser costoso para algunos microcontroladores. Se puede meter duration / 58 que es una aproximación suficiente y más fácil de hacer. De hecho, no es necesario transformar a cm, se puede trabajar directamente con el valor del tiempo.

La lectura de los sensores no es muy precisa por las propiedades del sonido. Por un lado, los objectos excesivamente blandos, pueden absorber el sonido y los objectos lisos y reflectantes del sonido pueden hacer que rebote en otra dirección y no sea detectado por el sensor. Estos son los falsos negativos.

También pueden ocurrir falsos positivos si se tarda demasiado poco en hacer otra medición y detecta el sonido rebotado de la anterior o de otro sensor. En nuestro caso hay tres sensores y puede que los triggers de unos afecten a otros. En este sensor se recomienda no hacer una medición hasta al menos 50ms después.

Para mejorar y entender los sensores hay que pensar en el ángulo efectivo de los mismos. Este es de unos 55º, aunque es más preciso por el centro a medida que el obstáculo está más lejos. De esta manera, es capaz de captar obstáculos a una distancia de 1.5m en una amplitud de aproximadamente 1m. Se pueden producir falsos negativos a poca distancia por el ángulo ciego del sensor y a mucha distancia se pueden producir falsos positivos al detectar objetos que no están en el camino recto del robot.

Mover el Robot

En nuestro proyecto, los motores se mueven con el Arduino Motor Shield rev3. Aquí hay un ejemplo de cómo mover los motores adelante y atrás:

void setup() {
  //Setup Channel A
  pinMode(12, OUTPUT); //Initiates Motor Channel A pin
  pinMode(9, OUTPUT); //Initiates Brake Channel A pin
  //Setup Channel B
  pinMode(13, OUTPUT); //Initiates Motor Channel A pin
  pinMode(8, OUTPUT);  //Initiates Brake Channel A pin
}

void loop(){
  //Motor A forward @ full speed
  digitalWrite(12, HIGH); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A at full speed
  //Motor B backward @ half speed
  digitalWrite(13, LOW);  //Establishes backward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 123);    //Spins the motor on Channel B at half speed

  delay(3000);

  digitalWrite(9, HIGH);  //Engage the Brake for Channel A
  digitalWrite(8, HIGH);  //Engage the Brake for Channel B

  delay(1000);
 
  //Motor A backward @ full speed
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 123);    //Spins the motor on Channel A at half speed
  //Motor B forward @ full speed
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 255);   //Spins the motor on Channel B at full speed
 
  delay(3000);
 
  digitalWrite(9, HIGH);  //Engage the Brake for Channel A
  digitalWrite(8, HIGH);  //Engage the Brake for Channel B
 
  delay(1000);
}

Funciones

La programación de este robot puede ser muy larga y complicada. Hay algunas cosas que se hacen siempre igual y que no es necesario volver a copiar y pegar el código. Por ejemplo, la lectura de los sensores es igual o el movimiento del robot.

En el ejemplo de los sensores hay una función para leer de los sensores:

int ping(int trig, int echo){
  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  int duration = pulseIn(echo,HIGH);
  int distance = duration * 0.034 / 2;
  return(distance);
}

También se puede hacer una función para moverse:

void front(){
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 123);    //Spins the motor on Channel A at half speed
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 123);   //Spins the motor on Channel B at full speed
}
void left(){
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 123);    //Spins the motor on Channel A at half speed
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, HIGH);   //Disengage the Brake for Channel B
  analogWrite(11, 0);      // <-- Stop the B motor
}
void right(){
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, HIGH);   //Disengage the Brake for Channel A
  analogWrite(3, 0);    // <-- Stops the A motor
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 123);     
}

Incluso se podría hacer que la función recibiera el tiempo que deseamos que esté moviendose y la velocidad:

void front(int time, int speed){
  digitalWrite(12, LOW);  //Establishes backward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, speed);    //Spins the motor on Channel A at half speed
  digitalWrite(13, HIGH); //Establishes forward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, speed);   //Spins the motor on Channel B at full speed
  delay(time);
}

Cambiar de dirección en función de los sensores

Una vez ya sabemos cómo mover el robot y leer de los sensores, debemos saber utilitzar la información de los sensores para decidir la dirección del robot.

Primera aproximación: Ir hacia el lado con más distancia

La primera aproximación a esto es ir hacia el lado que más distancia lee del robot. Más adelante ya se intentará que sea un poco más inteligente. Para ello, hay que modificar la función loop() para llamar a las funciones de lectura de los sensores:

void loop()
{
  distiz = ping(trigiz,echoiz);
  Serial.print("Izquierda: "); Serial.print(distiz);
   delay(50);
  distcen = ping(trigcen,echocen);
  Serial.print(" Centro: "); Serial.print(distcen);
  delay(50);
  distder = ping(trigder,echoder);
  Serial.print(" Derecha: "); Serial.println(distder);
  delay(50);
  if(distiz > distcen && distiz > distder){
   left(0,130);  // Tiempo y velocidad  
  }
  if(distder > distcen && distder > distiz){
   right(0,130);  
  }
  if(distcen > distder && distcen > distiz){
   front(0,130);  
  }
}

El problema es que cuando el robot tiene un obstáculo, por ejemplo, a su derecha, pero está acercándose en diagonal al mismo.

Arduino sensors 1.png

En este caso, detecta que la distancia de su derecha es menor que la central. De hecho, el sensor del centro está calculando distancias muy grandes porque el sonido rebota y se pierde. Por tanto, decide ir al centro aunque, evidentemente, la distancia a la izquierda es mayor.

Para evitar este fallo, podemos ir hacia el lado contrario al que menos distancia detecte. Esta solución mejora bastante el comportamiento del robot, aunque surge la duda de qué lado es el contrario del centro y el robot tiene, a veces, movimientos erráticos o demasiado cambiantes. Para tener una solución más óptima siguiendo este razonamiento, debemos plantear la siguiente estrategia:

Segunda aproximación: Distancia mínima y corregir el rumbo:

En primer lugar, hay que establecer una distancia mínima permisible. Si esta distancia es alcanzada por alguno de los sensores, debe corregirse el rumbo en dirección contraria. En el caso contrario, puede ir hacia delante o hacia donde más distancia detecte.

Algoritmo arduino robot1.png

volatile float DistanciaMaximaCentro = 25.00;
volatile float DistanciaMaximaIzquierda, DistanciaMaximaDerecha = 20.00;

void loop() {
  distcen = ping(trigcen,echocen);        // Distancia al centro
  if (distcen < DistanciaMaximaCentro) {  // 25 en el centro i 20  los lados
    Serial.println("Demasiado cerca");    // Todo esto sólo pasa cuando está muy cerca del centro
    distder = ping(trigder,echoder);
    delay(50);
    distiz = ping(trigiz,echoiz);
    delay(50);
    if (distiz < distder)  // Corrige el rumbo hacia el lado con más distancia 
      right(0,130);
    else if (distiz > distder) {
      left(0,130);
    }
  }
  else {
    Serial.println("OK");   // Casi siempre pasará esto, la distancia es mayor que la mínima
    front(0,130);
  }
  // Ahora va a comprobar cada lado.
  // Es posible que decida ir por el centro, pero que un lado esté peligrosamente cerca. 
  // En ese caso, debe corregir el rumbo.

  distiz = ping(trigiz,echoiz);
  if (distiz < DistanciaMaximaIzquierda) {  // Si está demasiado cerca por la izquierda
    Serial.println("Left too close");
    delay(50);
    distiz = ping(trigiz,echoiz);
    delay(50);
    distder = ping(trigder,echoder);
    delay(50);
    if (distiz > distder)
      front(0,130);
    else if (distiz < distder) {
      right(0,130);
    }
  }
 
  distder = ping(trigder,echoder);
  if (distder < DistanciaMaximaDerecha) {  // Si está demasiado cerca por la derecha
    Serial.println("Right too close");
    delay(50);
    distder = ping(trigder,echoder);
    delay(50);
    distiz = ping(trigiz,echoiz);
    delay(50);
    if (distder > distiz)
      front(0,130);
    else if (distder < distiz) {
      left(0,130);
    }
  }
}

Con este algoritmo el robot tiende a ir hacia el centro. El algoritmo anterior no tenía ninguna prioridad una dirección u otra. Esto hace que el movimiento sea más estable. El robot irá en una dirección hasta que esté cerca de un obstáculo. En ese caso, el robot irá corrigiendo el rumbo poco a poco en cada iteración hasta que dé con una dirección en la que ningún sensor esté demasiado cerca.

Este es el programa completo:

//Declaración de las variables de los pines:
int trigiz = 2; 
int echoiz = 4; 
int trigcen = 5;
int echocen = 6;
int trigder = 7; 
int echoder = 10; 
int distiz, distcen, distder; //Las variables de las distancias

volatile float DistanciaMaximaCentro = 25.00;
volatile float DistanciaMaximaIzquierda = 20.00;
volatile float DistanciaMaximaDerecha = 20.00;

void setup()
{
 pinMode(trigiz , OUTPUT);  // Los trig son de salida
 pinMode(trigcen , OUTPUT);
 pinMode(trigder , OUTPUT);
 pinMode(echoiz , INPUT);   // Los echo son de entrada
 pinMode(echocen , INPUT);
 pinMode(echoder , INPUT);
 Serial.begin(9600);
 //Setup Channel A
 pinMode(12, OUTPUT); //Inicia el pin del Motor A
 pinMode(9, OUTPUT); //Inicia el freno del Motor A
 //Setup Channel B
 pinMode(13, OUTPUT); //Inicia el pin del Motor B
 pinMode(8, OUTPUT);  //Inicia el freno del Motor B 
}

int ping(int trig, int echo){
  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  float duration = pulseIn(echo,HIGH);
  int distance = duration * 0.034 / 2;
  return(distance);
}

void front(int time, int speed){
  digitalWrite(12, HIGH);  // Dirección de A
  digitalWrite(9, LOW);   // Quitar el freno de A
  analogWrite(3, speed);    // Establece la velocidad de A
  digitalWrite(13, LOW); // Dirección de B
  digitalWrite(8, LOW);   // Quita el freno de B
  analogWrite(11, speed);   // Establece velocidad de B
    delay(time);
}
void left(int time, int speed){
  digitalWrite(12, HIGH);   
  digitalWrite(9, LOW);   
  analogWrite(3, speed);   
  digitalWrite(13, LOW); 
  digitalWrite(8, HIGH);   
  analogWrite(11, 0);      // <-- Parar el motor B (velocidad 0)
    delay(time);
}
void right(int time, int speed){
  digitalWrite(12, HIGH);  
  digitalWrite(9, HIGH);   
  analogWrite(3, 0);    // <-- Para el motor A
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed);    
    delay(time); 
}

void loop() {
  distcen = ping(trigcen,echocen);        // Distancia al centro
  if (distcen < DistanciaMaximaCentro) {  // 25 en el centro i 20  los lados
    Serial.print("Demasiado cerca: "); Serial.println(distcen);   // Todo esto sólo pasa cuando está muy cerca del centro
    distder = ping(trigder,echoder);
    delay(50);
    distiz = ping(trigiz,echoiz);
    delay(50);
    if (distiz < distder)  // Corrige el rumbo hacia el lado con más distancia 
      right(0,130);
    else if (distiz > distder) {
      left(0,130);
    }
  }
  else {
    Serial.println("OK");   // Casi siempre pasará esto, la distancia es mayor que la mínima
    front(0,130);
  }
  // Ahora va a comprobar cada lado.
  // Es posible que decida ir por el centro, pero que un lado esté peligrosamente cerca. 
  // En ese caso, debe corregir el rumbo.

  distiz = ping(trigiz,echoiz); 
  if (distiz < DistanciaMaximaIzquierda) {  // Si está demasiado cerca por la izquierda
    Serial.print("Left too close: "); Serial.print(distiz);
    delay(50);
    distcen = ping(trigcen,echocen); 
    delay(50);
    distder = ping(trigder,echoder);
    delay(50);
    if (distcen > distder)
      { front(100,130); Serial.println(" To front ");}  // 100ms para que corrija el rumbo durante más tiempo
    else if (distiz < distder) {
      { right(100,130); Serial.println(" To right ");} // 100ms para que corrija el rumbo durante más tiempo
    }
  }
 
  distder = ping(trigder,echoder);
  if (distder < DistanciaMaximaDerecha) {  // Si está demasiado cerca por la derecha
    Serial.print("Right too close: "); Serial.print(distder);
    delay(50);
    distcen = ping(trigcen,echocen); 
    delay(50);
    distiz = ping(trigiz,echoiz);
    delay(50);
    if (distcen > distiz)
      { front(100,130); Serial.println(" To front ");}
    else if (distder < distiz) {
       { left(100,130); Serial.println(" To left ");}
    }
  }
}

Refinando el algoritmo:

Si hemos ido mirando el monitor del puerto serie, habremos visto que a veces tiene lecturas muy extremas cuando no funciona del todo bien el sensor. Si nos fiamos de una sola lectura, puede cambiar el rumbo hacia el lugar que no nos interesa. Por eso, podemos sacar la media de varias lecturas, de manera que se suavizan los valores extremos. Para hacerlo, podem usar una media móvil, de manera que los valores anteriores influyen en la distáncia media actual.

La media móvil tiene en cuenta unos cuantos valores anteriores.

 Lectura de sensores	  200	200	200	200	7	200	200	7	7	5
 Media móvil N=3	 		200	200	135	135	135	135	71	6

Sin la media móvil, el robot pararía en la lectura 7, pero no para. Luego cuando ya lee 7,7 y 5, baja la media y para.

Para poder hacer la media móvil, la manera más fácil es con un buffer circular para el que hay librerías (Tutorial). Aunque También podemos usar una técnica llamada Media exponencial móbil, la cual da más importancia a los datos nuevos o recientes. Para la EWMA hay bibliotecas (EWMA) para arduino. (Tutorial interesante en español). La ventaja de EWMA sobre la media móvil es que computacionalmente es muy eficiente, ya que sólo tiene en cuenta el último valor y el actual y hace una simple operación aritmética.

En cualquier caso, hay lecturas demasiado extremas con valores negativos o demasiado altos. En ese caso, se puede establecer un máximo y un mínimo aceptable como medición y repetir la medición las veces que haga falta hasta que salga un valor aceptable con el que luego sacar la media móvil.

En el siguiente ejemplo, símplemente usamos la biblioteca Ewma para obtener valores más suaves:

#include <Ewma.h>
#include <EwmaT.h>

int trig = 2;
int echo = 4;
int dist; //Las variables de las distancias
int distf;
Ewma * filtro; // Declaración del objeto que va a filtrar

void setup()
{
 pinMode(trig , OUTPUT);
 pinMode(echo , INPUT);
 Serial.begin(9600);
 filtro = new Ewma(0.5); // El valor entre [0,1], a más valor menos se aplica el filtro al valor actual.
// Con 0, sólo cuenta el valor inicial, con 0,5 tarda 4 o 5 lecturas en adoptar el valor actual.
} 
int ping(int trig, int echo){
  digitalWrite(trig,LOW);  
  delayMicroseconds(2);    // Deja en LOW 2 microsegundos para "resetar" el sensor
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);   // Ponemos el trig a HIGH durante 10 microsegundos, el sensor enviará luego 8 pulsos
  digitalWrite(trig,LOW);
  float duration = pulseIn(echo,HIGH); // Capta el tiempo que pasa hasta que retornan los pulsos.
  int distance = duration * 0.034 / 2; // Cálculo de la distancia en cm
  return(distance);
}

void loop()
{
  dist = ping(trig,echo);
  distf = (int) filtro->filter(dist); // Aplicamos el filtro
  Serial.print(" Distancia: "); Serial.print(dist); Serial.print(" Filtro: "); Serial.println(distf);
  delay(50);
}

Así, el algoritmo completo del robot con un filtro de média móvil queda:

#include <Ewma.h>
#include <EwmaT.h>

//Declaración de las variables de los pines:
int trigiz = 2; 
int echoiz = 4; 
int trigcen = 5;
int echocen = 6;
int trigder = 7; 
int echoder = 10; 
int distiz, distcen, distder; //Las variables de las distancias
Ewma * filtroI;
Ewma * filtroD;
Ewma * filtroC;
double alpha;

volatile float DistanciaMaximaCentro = 25.00;
volatile float DistanciaMaximaIzquierda = 20.00;
volatile float DistanciaMaximaDerecha = 20.00;

void setup()
{
 pinMode(trigiz , OUTPUT);  // Los trig son de salida
 pinMode(trigcen , OUTPUT);
 pinMode(trigder , OUTPUT);
 pinMode(echoiz , INPUT);   // Los echo son de entrada
 pinMode(echocen , INPUT);
 pinMode(echoder , INPUT);
 Serial.begin(9600);
 //Setup Channel A
 pinMode(12, OUTPUT); //Inicia el pin del Motor A
 pinMode(9, OUTPUT); //Inicia el freno del Motor A
 //Setup Channel B
 pinMode(13, OUTPUT); //Inicia el pin del Motor B
 pinMode(8, OUTPUT);  //Inicia el freno del Motor B
 alpha = 0.5;
 filtroI = new Ewma(alpha);  
 filtroD = new Ewma(alpha);  
 filtroC = new Ewma(alpha);  
}

int ping(int trig, int echo){
  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  float duration = pulseIn(echo,HIGH);
  int distance = duration * 0.034 / 2;
  return(distance);
}

void front(int time, int speed){
  digitalWrite(12, HIGH);  // Dirección de A
  digitalWrite(9, LOW);   // Quitar el freno de A
  analogWrite(3, speed);    // Establece la velocidad de A
  digitalWrite(13, LOW); // Dirección de B
  digitalWrite(8, LOW);   // Quita el freno de B
  analogWrite(11, speed);   // Establece velocidad de B
    delay(time);
}
void left(int time, int speed){
  digitalWrite(12, HIGH);   
  digitalWrite(9, LOW);   
  analogWrite(3, speed);   
  digitalWrite(13, LOW); 
  digitalWrite(8, HIGH);   
  analogWrite(11, 0);      // <-- Parar el motor B (velocidad 0)
    delay(time);
}
void right(int time, int speed){
  digitalWrite(12, HIGH);  
  digitalWrite(9, HIGH);   
  analogWrite(3, 0);    // <-- Para el motor A
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed);    
    delay(time); 
}

void loop() {
  distcen = (int) filtroC->filter(ping(trigcen,echocen)); // Distancia al centro (con filtro)
  if (distcen < DistanciaMaximaCentro) {  // 25 en el centro i 20  los lados
    Serial.print("Demasiado cerca: "); Serial.println(distcen);   // Todo esto sólo pasa cuando está muy cerca del centro
    distder = (int) filtroD->filter(ping(trigder,echoder));
    delay(50);
    distiz = (int) filtroI->filter(ping(trigiz,echoiz));
    delay(50);
    if (distiz < distder)  // Corrige el rumbo hacia el lado con más distancia 
      right(0,130);
    else if (distiz > distder) {
      left(0,130);
    }
  }
  else {
    Serial.println("OK");   // Casi siempre pasará esto, la distancia es mayor que la mínima
    front(0,130);
  }
  // Ahora va a comprobar cada lado.
  // Es posible que decida ir por el centro, pero que un lado esté peligrosamente cerca. 
  // En ese caso, debe corregir el rumbo.

  distiz = (int) filtroI->filter(ping(trigiz,echoiz)); 
  if (distiz < DistanciaMaximaIzquierda) {  // Si está demasiado cerca por la izquierda
    Serial.print("Left too close: "); Serial.print(distiz);
    delay(50);
    distcen = (int) filtroC->filter(ping(trigcen,echocen));
    delay(50);
    distder = (int) filtroD->filter(ping(trigder,echoder));
    delay(50);
    if (distcen > distder)
      { front(100,130); Serial.println(" To front ");}  // 100ms para que corrija el rumbo durante más tiempo
    else if (distiz < distder) {
      { right(100,130); Serial.println(" To right ");} // 100ms para que corrija el rumbo durante más tiempo
    }
  }
 
  distder = (int) filtroD->filter(ping(trigder,echoder));
  if (distder < DistanciaMaximaDerecha) {  // Si está demasiado cerca por la derecha
    Serial.print("Right too close: "); Serial.print(distder);
    delay(50);
    distcen = (int) filtroC->filter(ping(trigcen,echocen));
    delay(50);
    distiz = (int) filtroI->filter(ping(trigiz,echoiz));
    delay(50);
    if (distcen > distiz)
      { front(100,130); Serial.println(" To front ");}
    else if (distder < distiz) {
       { left(100,130); Serial.println(" To left ");}
    }
  }
}

Seguir un camino

En los ejemplos anteriores, el robot consigue moverse "aleatoriamente" sin chocar. Ahora vamos a ser un poco más ambiciosos y vamos a modificarlo para que pueda llegar al final de un camino estrecho y con curvas.

Nuestro camino no va a tener vifurcaciones ni la necesidad de resolver un laberinto. Tan solo necesitamos que no vuelva atrás y que siga lo más rápido posible el camino.

La estratégia será distinta a sólo evitar obstáculos. En nuestro caso lo mejor es seguir el camino. Como un camino tiene dos paredes a los lados, si las seguimos, en teoria vamos a llegar al final. Así, vamos a imaginar que estamos en un pasillo muy largo, con curvas y a oscuras. La mejor manera de salir es poner una mano en una pared y seguirla, con la otra mano delante para evitar chocar.

Cami.png

Por tanto, el robot debe tomar como referencia una pared y mantener una distancia estable respecto a esta. Al mismo tiempo, evitará chocar contra la otra pared si se estrecha y evitará chocar contra el frente si la curva es muy cerrada. Este algoritmo se puede encontrar como un robot "Follow Wall". Aunque se podría mejorar intentando que la distancia a la pared izquierda y derecha fuera casi la misma, de manera que el robot pueda ir por el centro. Por último, cabe la posibilidad de llegar a una esquina muy cerrada, por lo que tendrá que dar media vuelta o un cuarto de vuelta y volver a evaluar.

Este es, de forma simple, es algoritmo si decidimos que su pared de referencia es la derecha:

Diagrama sigue paredes.png

Y esta es la primera versión funcional del mismo:

//Declaración de las variables de los pines:
int trigiz = 2; 
int echoiz = 4; 
int trigcen = 5;
int echocen = 6;
int trigder = 7; 
int echoder = 10; 
int distiz, distcen, distder; //Las variables de las distancias

volatile float DistanciaMinimaCentro = 25.00;
volatile float DistanciaMinimaIzquierda = 20.00;
volatile float DistanciaMaximaDerecha = 20.00;
volatile float DistanciaMinimaDerecha = 10.00;

int LED = A5;  // Este led puede avisar de cosas, en este caso, indica que está dando media vuelta.

void setup()
{
  pinMode(LED , OUTPUT);
 pinMode(trigiz , OUTPUT);  // Los trig son de salida
 pinMode(trigcen , OUTPUT);
 pinMode(trigder , OUTPUT);
 pinMode(echoiz , INPUT);   // Los echo son de entrada
 pinMode(echocen , INPUT);
 pinMode(echoder , INPUT);
 Serial.begin(9600);
 //Setup Channel A
 pinMode(12, OUTPUT); //Inicia el pin del Motor A
 pinMode(9, OUTPUT); //Inicia el freno del Motor A
 //Setup Channel B
 pinMode(13, OUTPUT); //Inicia el pin del Motor B
 pinMode(8, OUTPUT);  //Inicia el freno del Motor B 
}

int ping(int trig, int echo){
  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  float duration = pulseIn(echo,HIGH);
  int distance = duration * 0.034 / 2;
  return(distance);
}

void front(int time, int speed){
  digitalWrite(12, HIGH);  // Dirección de A
  digitalWrite(9, LOW);   // Quitar el freno de A
  analogWrite(3, speed);    // Establece la velocidad de A
  digitalWrite(13, LOW); // Dirección de B
  digitalWrite(8, LOW);   // Quita el freno de B
  analogWrite(11, speed);   // Establece velocidad de B
    delay(time);
}
void left(int time, int speed){
  digitalWrite(12, HIGH);   
  digitalWrite(9, LOW);   
  analogWrite(3, speed);   
  digitalWrite(13, LOW); 
  digitalWrite(8, HIGH);   
  analogWrite(11, 0);      // <-- Parar el motor B (velocidad 0)
    delay(time);
}
void right(int time, int speed){
  digitalWrite(12, HIGH);  
  digitalWrite(9, HIGH);   
  analogWrite(3, 0);    // <-- Para el motor A
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed);    
    delay(time); 
}
void turn(int time, int speed){
  digitalWrite(12, LOW);  
  digitalWrite(9, LOW);   
  analogWrite(3, speed);  
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed);    
    delay(time); 
}

void loop() {
 digitalWrite(LED, LOW); 
  distcen = ping(trigcen,echocen);        // Distancia al centro
  if (distcen < DistanciaMinimaCentro) {  // 25 en el centro
    Serial.print("Demasiado cerca: "); Serial.println(distcen);   // Todo esto sólo pasa cuando está muy cerca del centro
    distder = ping(trigder,echoder);
    delay(50);
    distiz = ping(trigiz,echoiz);
    delay(50);
    if (distder > DistanciaMinimaDerecha) {  // Corrige el rumbo hacia la derecha si puede
      right(0,130); 
      Serial.print("Corrijo a la derecha: "); Serial.println(distder);
      }
    else if (distiz > DistanciaMinimaIzquierda) {  // Corrige el rumbo a la izquierda
      left(0,130);
      Serial.print("Corrijo a la izquierda: "); Serial.println(distiz);
    }
    else {  digitalWrite(LED, HIGH); turn(500,130); Serial.println("Media Vuelta");} // Si todos son demasiado cortos se da la vuelta
  }
  else {
    Serial.println("OK");   // Casi siempre pasará esto, la distancia es mayor que la mínima
    front(0,130);
  }
  // Ahora va a comprobar la distancia a la derecha
  // Es posible que decida ir por el centro, pero ha de mantener una distancia estable a la derecha
  // En ese caso, debe corregir el rumbo.

  distder = ping(trigder,echoder);
  delay(50);
  if (distder < DistanciaMinimaDerecha) {  // Si está demasiado cerca por la derecha
    Serial.print("Derecha demasiado cerca: "); Serial.print(distder);
      distiz = ping(trigiz,echoiz);
      delay(50);
      if(distiz > DistanciaMinimaIzquierda)  { 
        left(100,130); Serial.println(" Corrijo a la izquierda "); }  // Corrige a la izquierda si puede
    }
    else if (distder > DistanciaMaximaDerecha) { // Demasiado lejos
        Serial.print("Derecha demasiado lejos: "); Serial.print(distder);
        right(100,130); Serial.println(" Corrijo a la derecha");
      }
  }

Mejoras:

Al algoritmo anterior funciona bien casi siempre y para salir de un camino puede que sea suficiente. No obstante, podemo detectar algun comportamiento extraño o demasiado brusco. A continuación vamos a mejorar algunas cosas:

En primer lugar, el robot tiene un rango de distancia máxima y mínima a la derecha. Esto hace que no siempre esté corrigiendo el rumbo, sólo cuando se sale del rango, no obstante, esta corrección es un poco brusca. Podemos establecer una corrección más suave con un rango intermedio que gire el robot cambiando la velocidad de la rueda contraria sin dejar de avanzar.

Cuando el robot (En realidad el sensor derecho) está en la franja verde va hacia delante, si está en la franja naranja, corrige el rumbo suavemente, cuando llega a la roja, lo corrige frenando la rueda contraria.

Para ello, vamos a declarar otras variables entre 10 y 20 cm que indiquen cuando va a empezar a corregir el rumbo suavemente. También debemos añadir a las funciones de left() i right() un factor de corrección de manera que indique si la velocidad de la rueda contraria debe ser 0 o un poco más.

También podemos hacer que esa corrección modifique las velocidades de forma proporcional a las distancias. De esta forma, desde la distancia en que comienza a corregir el rumbo hasta la que debe girar completamente, va corrigiendo gradualmente.

Aquí tenemos un algoritmo que funciona bastante bien con el cambio proporcional. También hemos cambiado la decisión cuando va chocar de frente para que decida ir a la mayor distancia posible.

//Declaración de las variables de los pines:
int trigiz = 2; 
int echoiz = 4; 
int trigcen = 5;
int echocen = 6;
int trigder = 7; 
int echoder = 10; 
int distiz, distcen, distder; //Las variables de las distancias

volatile float DistanciaMinimaCentro = 25.00;
volatile float DistanciaMinimaIzquierda = 10.00;
volatile float DistanciaIIzquierda = 15.00;
volatile float DistanciaMaximaDerecha = 20.00; // no debe pasar de ahí
volatile float DistanciaSDerecha = 16.00; // A partir de ahí va corrigiendo el rumbo
volatile float DistanciaMinimaDerecha = 10.00; // Debe alejarse
volatile float DistanciaIDerecha = 14.00; // A partir de ahí va corrigiendo el rumbo

int LED = A5;  // Este led puede avisar de cosas, en este caso, indica que está dando media vuelta.

void setup()
{
  pinMode(LED , OUTPUT);
 pinMode(trigiz , OUTPUT);  // Los trig son de salida
 pinMode(trigcen , OUTPUT);
 pinMode(trigder , OUTPUT);
 pinMode(echoiz , INPUT);   // Los echo son de entrada
 pinMode(echocen , INPUT);
 pinMode(echoder , INPUT);
 Serial.begin(9600);
 //Setup Channel A
 pinMode(12, OUTPUT); //Inicia el pin del Motor A
 pinMode(9, OUTPUT); //Inicia el freno del Motor A
 //Setup Channel B
 pinMode(13, OUTPUT); //Inicia el pin del Motor B
 pinMode(8, OUTPUT);  //Inicia el freno del Motor B 
}

int ping(int trig, int echo){
  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  float duration = pulseIn(echo,HIGH);
  int distance = duration * 0.034 / 2;
  return(distance);
}

void front(int time, int speed){
  digitalWrite(12, HIGH);  // Dirección de A
  digitalWrite(9, LOW);   // Quitar el freno de A
  analogWrite(3, speed);    // Establece la velocidad de A
  digitalWrite(13, LOW); // Dirección de B
  digitalWrite(8, LOW);   // Quita el freno de B
  analogWrite(11, speed);   // Establece velocidad de B
    delay(time);
}
void left(int time, int speed,float factor){  // Un factor 0 gira del todo y un factor 1 nada
  digitalWrite(12, HIGH);   
  digitalWrite(9, LOW);   
  analogWrite(3, speed);   
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed*factor);      // <-- Reduce el motor B
    delay(time);
}
void right(int time, int speed, float factor){
  digitalWrite(12, HIGH);  
  digitalWrite(9, LOW);   
  analogWrite(3, speed*factor);    // <-- Reduce el motor A
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed);    
    delay(time); 
}
void turn(int time, int speed){
  digitalWrite(12, LOW);  
  digitalWrite(9, LOW);   
  analogWrite(3, speed);  
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed);    
    delay(time); 
}

void loop() {
  
 digitalWrite(LED, LOW);
  
  distcen = ping(trigcen,echocen);        // Distancia al centro
  delay(50);

   
  if (distcen < DistanciaMinimaCentro) {  // 25 en el centro
 //   Serial.print("Demasiado cerca: "); Serial.print(distcen);   // Todo esto sólo pasa cuando está muy cerca del centro
      digitalWrite(LED, HIGH);
      distder = ping(trigder,echoder);    // Distancia Derecha
      delay(50);
      distiz = ping(trigiz,echoiz);       // Distancia Izquierda
      delay(50);
    if (distder > distiz && distder > DistanciaMinimaDerecha)
    {
      right(100,130,0); 
   //   Serial.print("Corrijo a la derecha: "); Serial.println(distder);
    }
      else if (distder < distiz && distiz > DistanciaMinimaIzquierda){  // Corrige el rumbo a la izquierda
      left(100,130,0);
 //     Serial.print("Corrijo a la izquierda: "); Serial.println(distiz);
    }
    else {   turn(500,130); Serial.println("Media Vuelta");} // Si todos son demasiado cortos se da la vuelta
  }
  else {
    Serial.println("OK");   // Casi siempre pasará esto, la distancia es mayor que la mínima
    front(0,130);
  
  // Ahora va a comprobar la distancia a la derecha
  // Es posible que decida ir por el centro, pero ha de mantener una distancia estable a la derecha
  // En ese caso, debe corregir el rumbo.

    distder = ping(trigder,echoder);    // Distancia Derecha
    delay(50);
    
 if (distder > DistanciaSDerecha) { // Lejos por la derecha, pero no demasiado
     //  Serial.print("Derecha lejos: "); Serial.print(distder);
       float factor = (DistanciaMaximaDerecha - distder)/( DistanciaMaximaDerecha - DistanciaSDerecha); 
       if (distder > DistanciaMaximaDerecha) factor = 0;
       
        // Factor proporcional a la diferencia entre la máxima i superior 

       right(0,130,factor);  Serial.print(" Corrijo  "); Serial.print(factor);Serial.println(" a la derecha ");
      }
  else if (distder < DistanciaIDerecha) {  // Si está cerca por la derecha (pero no al máximo)
     // Serial.print("Derecha cerca: "); Serial.print(distder);
      distiz = ping(trigiz,echoiz);       // Distancia Izquierda
      delay(50);
      if(distiz > DistanciaMinimaIzquierda)  { 
        float factor = (distder - DistanciaMinimaDerecha)/(DistanciaIDerecha -  DistanciaMinimaDerecha); 
        if (distder < DistanciaMinimaDerecha) factor=0;
        // Factor proporcional a la diferencia entre la mínima e inferior
        left(0,130,factor); Serial.print(" Corrijo  "); Serial.print(factor);Serial.println(" a la izquierda ");
         }  // Corrige a la izquierda si puede
    }
  
  distiz = ping(trigiz,echoiz);
  delay(50);
  if(distiz < DistanciaMinimaIzquierda && distder > DistanciaMinimaDerecha)  { 
        right(0,130,0);// Serial.println(" Corrijo a la derechaaa "); 
        }  // es preciso ir a la derecha para no chocar
  } // Del else del front
  }
Estas teóricas mejoras (las distancias intermedias y el factor de velocidad) en el algoritmo, en la práctica pueden empeorar el comportamiento del algoritmo si no las ajustamos muy bien, ya que ralentizan la toma de decisiones de Arduino y los motores tienen una velocidad mínima para moverse. Por tanto, puede que no sea muy adecuado esforzarse tanto en este aspecto y más en los siguientes.
Hay una mejora física a parte del algoritmo y consiste en poner el sensor derecho un poco más girado hacia la derecha. Esto evita lecturas extrañas al rebotar mal el sonido. Si va en línea recta, el sonido llega en un ángulo de 45 grados a la pared y puede rebotar mal i no volver. Si el ángulo és más cerrado, el sonido seguro que vuelve.

Otra mejora puede ser agregar el algoritmo EWMA a este algoritmo para que suavice las lecturas. Este es muy sencillo de usar, tan solo añadir la biblioteca necesaria y usar las lecturas como argumento de la función:

#include <Ewma.h>
#include <EwmaT.h>

//Declaración de las variables de los pines:
int trigiz = 2; 
int echoiz = 4; 
int trigcen = 5;
int echocen = 6;
int trigder = 7; 
int echoder = 10; 
int distiz, distcen, distder; //Las variables de las distancias
int d = 20; // Delay de los sensores
int tiempo = 0;

Ewma * filtroI;
Ewma * filtroD;
Ewma * filtroC;
double alpha;
float factor = 0.0;

float duration;
int distance;


volatile float DistanciaMinimaCentro = 25.00;
volatile float DistanciaMinimaIzquierda = 10.00;
volatile float DistanciaIIzquierda = 15.00;
volatile float DistanciaMaximaDerecha = 18.00; // no debe pasar de ahí
volatile float DistanciaSDerecha = 16.00; // A partir de ahí va corrigiendo el rumbo
volatile float DistanciaMinimaDerecha = 11.00; // Debe alejarse
volatile float DistanciaIDerecha = 14.00; // A partir de ahí va corrigiendo el rumbo

int LED = A5;  // Este led puede avisar de cosas, en este caso, indica que está dando media vuelta.

void setup()
{
  pinMode(LED , OUTPUT);
 pinMode(trigiz , OUTPUT);  // Los trig son de salida
 pinMode(trigcen , OUTPUT);
 pinMode(trigder , OUTPUT);
 pinMode(echoiz , INPUT);   // Los echo son de entrada
 pinMode(echocen , INPUT);
 pinMode(echoder , INPUT);
 Serial.begin(9600);
 //Setup Channel A
 pinMode(12, OUTPUT); //Inicia el pin del Motor A
 pinMode(9, OUTPUT); //Inicia el freno del Motor A
 //Setup Channel B
 pinMode(13, OUTPUT); //Inicia el pin del Motor B
 pinMode(8, OUTPUT);  //Inicia el freno del Motor B 
 alpha = 0.8;
 filtroI = new Ewma(alpha);  
 filtroD = new Ewma(alpha);  
 filtroC = new Ewma(alpha);  
}

int ping(int trig, int echo){
  digitalWrite(trig,LOW);
  delayMicroseconds(2);
  digitalWrite(trig,HIGH);
  delayMicroseconds(10);
  digitalWrite(trig,LOW);
  duration = pulseIn(echo,HIGH);
 // Serial.println(duration);
  distance = duration / 58; //* 0.034 / 2;
  return(distance);
}

void front(int time, int speed){
  digitalWrite(12, HIGH);  // Dirección de A
  digitalWrite(9, LOW);   // Quitar el freno de A
  analogWrite(3, speed);    // Establece la velocidad de A
  digitalWrite(13, LOW); // Dirección de B
  digitalWrite(8, LOW);   // Quita el freno de B
  analogWrite(11, speed);   // Establece velocidad de B
    delay(time);
}
void left(int time, int speed,float factor){  // Un factor 0 gira del todo y un factor 1 nada
  digitalWrite(12, HIGH);   
  digitalWrite(9, LOW);   
  analogWrite(3, speed);   
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed*factor);      // <-- Reduce el motor B
    delay(time);
}
void right(int time, int speed, float factor){
  digitalWrite(12, HIGH);  
  digitalWrite(9, LOW);   
  analogWrite(3, speed*factor);    // <-- Reduce el motor A
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed);    
    delay(time); 
}
void turn(int time, int speed){
  digitalWrite(12, LOW);  
  digitalWrite(9, LOW);   
  analogWrite(3, speed);  
  digitalWrite(13, LOW); 
  digitalWrite(8, LOW);   
  analogWrite(11, speed);    
    delay(time); 
}

void loop() {
  // En caso de querer saber cuanto tarda cada ciclo
 // int t = millis();
 // Serial.println(t-tiempo);
 // tiempo = t;
 digitalWrite(LED, LOW);
  
  //distcen = ping(trigcen,echocen);        // Distancia al centro
  distcen = (int) filtroC->filter(ping(trigcen,echocen)); // Distancia al centro (con filtro)
  delay(d);

   
  if (distcen < DistanciaMinimaCentro) {  // 25 en el centro
 //   Serial.print("Demasiado cerca: "); Serial.print(distcen);   // Todo esto sólo pasa cuando está muy cerca del centro
      digitalWrite(LED, HIGH);
      distder = (int) filtroD->filter(ping(trigder,echoder)); // Distancia Derecha
      delay(d);
       distiz = (int) filtroI->filter(ping(trigiz,echoiz));   // Distancia Izquierda
      delay(d);
    if (distder > distiz && distder > DistanciaMinimaDerecha)
    {
      right(100,130,0); 
   //   Serial.print("Corrijo a la derecha: "); Serial.println(distder);
    }
      else if (distder < distiz && distiz > DistanciaMinimaIzquierda){  // Corrige el rumbo a la izquierda
      left(100,130,0);
 //     Serial.print("Corrijo a la izquierda: "); Serial.println(distiz);
    }
    else {   turn(500,130); /*Serial.println("Media Vuelta");*/} // Si todos son demasiado cortos se da la vuelta
    // Como ha habido un cambio grande de orientación, volvemos a medir
   /* distcen = ping(trigcen,echocen);        // Distancia al centro
    delay(50);
    distder = ping(trigder,echoder);    // Distancia Derecha
    delay(50);
    distiz = ping(trigiz,echoiz);       // Distancia Izquierda
    delay(50);*/
  }
  else {
    //Serial.println("OK");   // Casi siempre pasará esto, la distancia es mayor que la mínima
    front(0,130);
  
  // Ahora va a comprobar la distancia a la derecha
  // Es posible que decida ir por el centro, pero ha de mantener una distancia estable a la derecha
  // En ese caso, debe corregir el rumbo.

    distder = (int) filtroD->filter(ping(trigder,echoder));  // Distancia Derecha
    delay(d);
    
 if (distder > DistanciaSDerecha) { // Lejos por la derecha, pero no demasiado
     //  Serial.print("Derecha lejos: "); Serial.print(distder);
       factor = (DistanciaMaximaDerecha - distder)/( DistanciaMaximaDerecha - DistanciaSDerecha); 
       if (distder > DistanciaMaximaDerecha) factor = 0;
       
        // Factor proporcional a la diferencia entre la máxima i superior 

       right(0,130,factor); // Serial.print(" Corrijo  "); Serial.print(factor);Serial.println(" a la derecha ");
      }
  else if (distder < DistanciaIDerecha) {  // Si está cerca por la derecha (pero no al máximo)
     // Serial.print("Derecha cerca: "); Serial.print(distder);
      distiz = (int) filtroI->filter(ping(trigiz,echoiz));       // Distancia Izquierda
      delay(d);
      if(distiz > DistanciaMinimaIzquierda)  { 
        factor = (distder - DistanciaMinimaDerecha)/(DistanciaIDerecha -  DistanciaMinimaDerecha); 
        if (distder < DistanciaMinimaDerecha) factor=0;
        // Factor proporcional a la diferencia entre la mínima e inferior
        left(0,130,factor); //Serial.print(" Corrijo  "); Serial.print(factor);Serial.println(" a la izquierda ");
         }  // Corrige a la izquierda si puede
    }
  
  distiz = (int) filtroI->filter(ping(trigiz,echoiz));       // Distancia Izquierda
  delay(d);
  if(distiz < DistanciaMinimaIzquierda && distder > DistanciaMinimaDerecha)  { 
        right(0,130,0);// Serial.println(" Corrijo a la derechaaa "); 
        }  // es preciso ir a la derecha para no chocar
  } // Del else del front
  }

Este ejemplo tiene algunas optimizaciones en el algoritmo vistas por prueba y error, en los límites y en el delay(), ya que le damos menos tiempo de delay y tarda menos en volver a hacer otra lectura. Se han optimizado otras cosas menores, pero todo con el objetivo de hacer la mayor cantidad de mediciones posibles. Por poner un ejemplo, en nuesto caso el robot tarda unos 80ms en hacer el ciclo si todos los sensores estan en los rangos aceptables, pero 300ms si la derecha está demasiado cerca o lejos. En nuestro caso, querer optimizar el movimiento del robot, ha reducido sustancialmente el rango aceptable, por lo que más veces tardará 300ms frente a 80ms. Esto decrementa el tiempo de respuesta, lo que nos lleva plantearnos si la idea de hacer un rango superior o inferior es buena o no.

Sin embargo, las pruebas han confirmado que el filtro EWMA suaviza el comportamiento del robot de forma efectiva. El factor lo hemos puesto alto (0.8), por lo que la media está más influenciada por la última lectura, esto parece ir mejor, ya que mejora el tiempo de respuesta. Hay que tener en cuenta que los sensores son realmente lentos y si tardamos, por ejemplo, 200ms en hacer un ciclo del loop, sólo estamos midiendo 5 veces por segundo, por lo que necesitar muchas lecturas para modificar la media puede ser contraproducente.

Por último, la mejora más importante que se puede hacer a este robot sin modificar su hardware, es el algoritmo de PID. Este analiza las lecturas y la dirección del robot para corregir el rumbo antes de alcanzar las distancias máximas o mínimas.

PID

El algoritmo final del programa anterior se ha ido complicando y, en realidad, no es más que una aproximación al PID. Ahora vamos a simplificar el algoritmo y, al mismo tiempo, añadir el PID.

Se recomienda leer la parte del PID del Robot sigue líneas con Arduino, ya que en este artículo no vamos más que a poner el código.

#include <Ewma.h>
#include <EwmaT.h>

//Declaración de las variables de los pines:
int trigiz = 2;
int echoiz = 4;
int trigcen = 5;
int echocen = 6;
int trigder = 7;
int echoder = 10;
int distiz, distcen, distder; //Las variables de las distancias
int d = 20; // Delay de los sensores
int tiempo = 0;

Ewma * filtroI;
Ewma * filtroD;
Ewma * filtroC;
double alpha;
float factor = 0.0;

float duration;
int distance;

int base_iz = 100; // Velocidades base
int base_der = 100;
int correccion = 0;  // factor de corrección de las velocidades
int error = 0; // El error que es calculado cada vez
int error_anterior = 0;
float Kp = 0;   // El factor proporcional, hay que ajustarlo en el valor ideal
float Ki = 0;   // El factor integral, hay que ajustarlo
float Kd = 0;   // El factor derivativo que hay que ajustar
int integral = 0; // la integral que va acumulando los errores
int derivativa = 0; // La derivativa calcula el incremento del error
int leftMotorSpeed; // las velocidades de los motores
int rightMotorSpeed;
int velocidad_recta;
int diferencia = 0; // La diferencia entre la velocidad base y la máxima para las rectas
float Kv; // Factor para la velocidad en recta
int vel_maxima = 255;
int vel_minima = 0;


volatile float DistanciaMinimaCentro = 25.00;
volatile float DistanciaMinimaIzquierda = 10.00;
volatile float DistanciaMinimaDerecha = 11.00; // Debe alejarse

volatile float DistanciaDerecha = 15.00; // A partir de ahí va corrigiendo el rumbo

int LED = A5;  // Este led puede avisar de cosas, en este caso, indica que está dando media vuelta.

void setup()
{
  pinMode(LED , OUTPUT);
  pinMode(trigiz , OUTPUT);  // Los trig son de salida
  pinMode(trigcen , OUTPUT);
  pinMode(trigder , OUTPUT);
  pinMode(echoiz , INPUT);   // Los echo son de entrada
  pinMode(echocen , INPUT);
  pinMode(echoder , INPUT);
  Serial.begin(9600);
  //Setup Channel A
  pinMode(12, OUTPUT); //Inicia el pin del Motor A
  pinMode(9, OUTPUT); //Inicia el freno del Motor A
  //Setup Channel B
  pinMode(13, OUTPUT); //Inicia el pin del Motor B
  pinMode(8, OUTPUT);  //Inicia el freno del Motor B
  alpha = 0.8;
  filtroI = new Ewma(alpha);
  filtroD = new Ewma(alpha);
  filtroC = new Ewma(alpha);
}

int ping(int trig, int echo) {
  digitalWrite(trig, LOW);
  delayMicroseconds(2);
  digitalWrite(trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  // Serial.println(duration);
  distance = duration / 58; //* 0.034 / 2;
  return (distance);
}

void front(int time, int speed) {
  digitalWrite(12, HIGH);  // Dirección de A
  digitalWrite(9, LOW);   // Quitar el freno de A
  analogWrite(3, speed);    // Establece la velocidad de A
  digitalWrite(13, LOW); // Dirección de B
  digitalWrite(8, LOW);   // Quita el freno de B
  analogWrite(11, speed);   // Establece velocidad de B
  delay(time);
}
void left(int time, int speed, float factor) { // Un factor 0 gira del todo y un factor 1 nada
  digitalWrite(12, HIGH);
  digitalWrite(9, LOW);
  analogWrite(3, speed);
  digitalWrite(13, LOW);
  digitalWrite(8, LOW);
  analogWrite(11, speed * factor);    // <-- Reduce el motor B
  delay(time);
}
void right(int time, int speed, float factor) {
  digitalWrite(12, HIGH);
  digitalWrite(9, LOW);
  analogWrite(3, speed * factor);  // <-- Reduce el motor A
  digitalWrite(13, LOW);
  digitalWrite(8, LOW);
  analogWrite(11, speed);
  delay(time);
}
void turn(int time, int speed) {
  digitalWrite(12, LOW);
  digitalWrite(9, LOW);
  analogWrite(3, speed);
  digitalWrite(13, LOW);
  digitalWrite(8, LOW);
  analogWrite(11, speed);
  delay(time);
}

void loop() {
  // En caso de querer saber cuanto tarda cada ciclo
  // int t = millis();
  // Serial.println(t-tiempo);
  // tiempo = t;

  digitalWrite(LED, LOW);
  delay(d);

  distcen = (int) filtroC->filter(ping(trigcen, echocen)); // Distancia al centro (con filtro)
  distder = (int) filtroD->filter(ping(trigder, echoder)); // Distancia Derecha
  distiz = (int) filtroI->filter(ping(trigiz, echoiz));



  if (distcen < DistanciaMinimaCentro) {  // 25 en el centro
    //   Serial.print("Demasiado cerca: "); Serial.print(distcen);   // Todo esto sólo pasa cuando está muy cerca del centro
    digitalWrite(LED, HIGH);
    if (distder > distiz && distder > DistanciaMinimaDerecha)
    {
      right(100, 130, 0);
    }
    else if (distder < distiz && distiz > DistanciaMinimaIzquierda) { // Corrige el rumbo a la izquierda
      left(100, 130, 0);
    }
    else {
      turn(500, 130); // Si todos son demasiado cortos se da la vuelta
    }
    // Como ha habido un cambio grande de orientación, volvemos a medir
  }
  else {
    error = distder - DistanciaDerecha;     // Para el control proporcional
    if ((error * integral) < 0)
      integral = 0; // A esto se le llama integral Windup.
    integral = integral + error; // La integral
    derivativa = error - error_anterior; // La derivativa
    correccion = Kp * error + Ki * integral + Kd * derivativa;
    velocidad_recta = diferencia * exp(-Kv * abs(Kp * error)); // a más error, mucha menos velocidad
    leftMotorSpeed = base_iz + velocidad_recta + correccion;
    rightMotorSpeed  = base_der + velocidad_recta - correccion;

    error_anterior = error;  // Para la derivada

    /////// La parte de frenar y limitar las velocidades
    if (leftMotorSpeed > vel_maxima) leftMotorSpeed = vel_maxima;
    if (rightMotorSpeed > vel_maxima) rightMotorSpeed = vel_maxima;
    digitalWrite(9, LOW); // Quitamos el freno antes de decidir si lo ponemos
    if (leftMotorSpeed <= vel_minima) {
      leftMotorSpeed = vel_minima;  // El freno
      digitalWrite(9, HIGH);
    }
    digitalWrite(8, LOW); // Quitamos el freno antes de decidir si lo ponemos
    if (rightMotorSpeed <= vel_minima) {
      rightMotorSpeed = vel_minima;  // El freno
      digitalWrite(8, HIGH);
    }
    analogWrite(3, rightMotorSpeed);    //Spins the motor on Channel A
    analogWrite(11, leftMotorSpeed);   //Spins the motor on Channel B
  } // Del else del front
}

Enlaces

Esquivar obstáculos:

Seguir paredes o resolver laberintos:

PID:

Ver también: