Robot Con Sistema De Control

Partes

Lo que se ocupa:

1. Un controlador S4A EDU (aunque sirve cualquiera). La ventaja de este controlador es que trae los drivers de motor DC incorporados
2. Cable f-f de 3 pines
3. Motores DC pequeños con caja de engranajes (microgear motors). Los motores que utilizo son de 140/270 revoluciones. Estos motores viene con agarradera para montar en chasis como se ve en la foto.
4. Sensor analógico infrarrojo. Yo utilizo estos de Sparkfun
5. Un soporte de 4 baterias AA como este.

Herramientas

1. Cautín
2. Desatornillador, alicate y cortador de cables
3. Impresora 3D (para imprimir el chasis y pieza del controlador)

Son dos piezas las que hay que imprimir. Yo lo imprimí con resolución baja de 300 micras y plástico PLA. Tal vez tengan que ajustar algo las medidas, o limar un poco el plástico para que todo ajuste bien. Los archivos 3D los pueden descargar de acá junto con todo el código del robot.

9 More Images

Ensamblado

1. Simplemente monte los motores como se observa en la foto.
2. Inserte las piezas 3d de soporte de baterías en el chasis
3. Lo sensores van en la parte delantera del robot, y se ajustan con gasas de plástico (ver foto)
4. Coloque el controlador con tornillos (M3) en la pieza impresa 3D, y el soporte de baterías como se muestra en la foto.

Conexiones

1. El S4A EDU viene listo para conectar las baterías, y para conectar los motores. Note como los cables van conectados. el MR y ML van al revés (note el cable café y rojo en l a foto)

2. Los sensores van conectados a los pines A0 (sensor izquierdo), A1 (centro) y A2 (sensor derecho)

Objetivo

Tenemos un robot que queremos se mueva en un pasillo, potencialmente un laberinto. El robot debe ser capaz de moverse en el laberinto sin chocar con las paredes y poder circular libremente.

¿Que detecta el robot?

• El robot lo equipamos con 3 sensores infrarrojos analógicos, que detectan la pared cuando está a una distancia menor de los 3 cm (aproximadamente).
• El sensor lee la información en 10 bits, es decir nos da un valor entre 0 y 1023,
• si el sensor está cerca de la pared, la lectura será cercana a 0 y si está a lejos la lectura será alta (cercana a 1023)
• La lectura del sensor es una medida de distancia del robot a la pared

¿Cómo puede responder el robot?

• El robot puede corregir su trayectoria cambiando la velocidad de sus motores izquierdo y derecho.
• Como se ve en la imagen animada, si el robot está cerca de la pared izquierda, entonces puede disminuir la velocidad del motor derecho para hacer que el robot se mueva a la derecha, alejandose de la pared izquierda, y viceversa.
• Cuando la pared está adelante del robot, el sensor delantero detecta la pared y el robot debe girar, para cambiar dirección

¿Cuanto debe cambiar la velocidad del motor?

Esta es la parte interesante:

• En principio, la velocidad del motor debe cambiar proporcionalmente a lo cerca que esté de la pared.
• Si la distancia a la pared de un lado es corta, entonces, la velocidad del motor opuesta debe ser baja
• También debemos considerar si el movimiento es hacia la pared, o alejándose de la pared. Para ello el robot debe recordar la medida anterior a la pared y calcular la pendiente, es decir, en el intervalo de tiempo, cual es la diferencia de las distancias a la pared.

Ahora si estamos listos para hacer el algoritmo.

El error

Cada sensor del robot lee la distancia a la pared. Cuando el robot está exactamente en el centro, la distancia a la pared derecha y a la pared izquierda debería ser la misma. Definamos e(t) como el error en el instante de tiempo t. Para ser exactos

e(t) = sensorR(t) - sensorL(t),

es decir resto la lectura del sensor derecho a la lectura del sensor izquierdo.

Como se muestra en la primera foto hay tres situaciones:

1. A, cuando el robot está en el centro, e(t) = 0.
2. B, cuando está hacia la izquierda e(t) es positivo
3. C, cuando el robot está a la derecha e(t) es negativo

Esto es importante, pues el signo del error e(t) nos indica la dirección del error. Esto lo vamos a utilizar en nuestra fórmula.

Lo que tenemos que lograr cuando controlamos el robot es garantizar de que el error sea siempre 0. Entonces nuestra meta de control es e(t) = 0.

Respuesta de robot

Nos interesa entonces que el movimiento de los motores sea proporcional a el error e(t). Si speedR(t) y speedL(t) es la velocidad del motor derecho e izquierdo respectivamente. entonces las velocidades en cada instante, debería estar definidas,

speedR(t) = speedR(t) + e(t)

speedL(t) = speedL(t) - e(t)

Entonces el error informa la velocidad del motor, y mientras que suma para el motor derecho, resta para el izquierdo.

Con estas fórmulas ya podemos hacer que el robot camine y cuando se acerque a las paredes, comienza a girar de manera proporcional a la distancia de la pared. Sin embargo, este mecanismo de control puede hacer que el robot comience a ir de pared a pared, nunca encontrando el centro (oscilando alrededor del centro). Para eso podemos mejorar la respuesta

Mejorando la Respuesta

En la segunda foto se muestra el movimiento del robot en dos situaciones,

• en A, el robot está cerca de la pared izquierda y corrige según el error, sin embargo se está moviendo hacia la pared. Lo sabemos pues al hacer la diferencia entre el error en el instante t, e(t) y el error en el instante anterior e(t-1), nos damos cuenta que el error a aumentado, a pesar de la corrección en el paso anterior.
• en B, el robot está cerca de la pared izquierda, sin embargo se aleja de la pared. Lo sabemos porque al hacer la diferencia de e(t) con el error tiempo anterior e(t-1), el resultado es negativo

Esta información también la podemos utilizar para de alguna manera reforzar el cambio proporcional en la velocidad de motores. Definiendo d(t) = sensorR(t) - sensorL(t), nuestra función de error ahora quedaría así:

e(t) = d(t)+ [d(t) - d(t-1)]

Noten como si d(t-1) > d(t) el signo es negativo, y entonces le resta a la respuesta de e(t). Es decir, dado que el robot ya va en la dirección adecuada, que el cambio proporcional no sea tan intenso.

Formula Final

Solamente nos falta una cosa. Queremos poder cambiar el "peso" que tiene cada término en la ecuación anterior, entonces simplemente agregamos dos constantes que vamos a llamar Kp y Kd. La ecuación queda:

e(t) = Kp* d(t)+ Kd* [d(t) - d(t-1)]

Esta es la función que vamos a utilizar para luego corregir la velocidad de los motores con estas dos:

speedR(t) = speedR(t) + e(t)
speedL(t) = speedL(t) - e(t)

Ahora lo que nos queda es escribir el código para el Arduino.

EXTRA: He creado una Google Sheets donde metí las fórmulas y donde pueden cambiar las constantes Kp y Kd para que vean el efecto en el control de movimiento

El código lo pueden descargar de este github. Acá también utilizo esta librería para controlar los motores con el S4A EDU.

De todas maneras acá les dejo el código. Las explicaciones están en los comentarios, y las fórmulas corresponden al paso anterior

// @author Tomas de Camino Beck
// Dic 2017
#include <robotkit.h></robotkit.h>

//define variables a utilizar
int error=0;
int dif,difAnt=0;
const float Kp=1.1;
const float Kd=1.3;
void setup() {
  //prepara los motores
  setMotors();

  //Prepara los pines para lectura
  pinMode(A0, INPUT);//sensor izquierdo
  pinMode(A1, INPUT);//sensor derecho
  pinMode(A2, INPUT);//sensor central
}

void loop() {
  //calcula la diferencia
  dif = analogRead(A0) - analogRead(A2);
  // calculo del error (se redondea)
  error = round(Kp*(dif)+Kd*(difAnt-dif));
  // para mantener en memoria la dif anterior
  difAnt=dif;
  //recalcula las velocidades de motores
  int speedL = constrain(255 - error, 0, 255);//velocidad izquierda
  int speedR = constrain(255 + error, 0, 255);//velcidad derecha
  //revisa el sensor central para dar vuelta
  if (analogRead(A1) < 980){
    stopMotors();
    delay(200);
    backward(150, 100);
    pivotLeft(200, 450);
  }
  //avanza hacia adelante con las velocidades calculadas
 // el tercer argumento es el tiempo que aplica esas velocidades
  forward(speedL, speedR ,5);

}

Ajuste de Kp y Kd

En el video se ve el robot funcionando. Sin embargo hay que ajustar los valores de Kp y Kd. Hay varias formas de hacerlo, sin embargo, para este caso es mejor de forma manual. Simplemente ajuste primero Kp, y cuando funcione de manera aceptable, comience a aumentar Kp. En principio comenzará a observar que el robot llega más rápido al centro sin oscilaciones.

En el video simplemente lo pongo a moverse por un pasillo. les queda a ustedes armar un laberinto

Retos

• Ajustar Kp y Kd no es sencillo. ¿Existirá una manera mejor de hacerlo?
• Agregarle información de el desempeño en el tiempo (la parte integral)
• Que se mueva por un laberinto
• ¿Qué pasa si le agrego sensores a 45 grados a cada lado? Note como en el diseño del chasis tinene donde colocarlos
• Pruebe algunos otros algoritmos de control

Material Adicional

• PID en Wikipedia
• Otro instructable de control PID bastante bueno (en ingles) de un seguidor de línea