Robot Balancin V: El controlador PID.

En esta ocasión veremos un poco de programación, probablemente el proceso más difícil de desarrollar para mi fue el Controlador PID (Proporcional, integral, Derivativo), pero por supuesto estoy dispuesto a compartir mi explicación  para ustedes de modo que les paresca lo mas sencillo posible, el controlador PID usa tres acciones distintas para mantener el error lo mínimo posible en un sistema inestable, nuestro sistema es el robot, el setpoint es el ángulo ideal, es decir el ángulo  en donde se encuentra en posición vertical, como ya saben el robot es incapaz de mantenerse en equilibrio sin mover sus llantas hacia un lado o hacia el otro, la señal PWM aumenta si la perturbación en el sistema es alta, y esta señal está determinada por el PID.

Fórmula del algoritmo PID.

Aunque la fórmula parezca intimidante escribirla en el programa solo requiere de unas cuantas líneas de código, veamos que representan las siglas del PID.


Proporcional: La acción proporcional representa la respuesta inmediata que debe tener el sistema sobre el error, esto significa que si el error es pequeño la acción proporcional será mínima, si el error es grande la respuesta también lo será, en mi programa esta escrito de la siguiente forma:

                                                           Error = SetpointA - AnguloA; 

Error representa la acción proporcional, SetpointA representa el ángulo ideal del robot, AnguloA representa el ángulo en el cual se encuentra el robot, de modo que si recibe una perturbación el error aumentará proporcionalmente. 


Integral: La acción Integral es la suma de los errores cuya función es reducir el error lo más rápido posible, supongamos que el robot se inclina X grados hacia una dirección, el error se irá acumulando en cada bucle del programa de modo que mientras más tiempo pase inclinado mayor será la acción integral, recordemos que el bucle del programa dura unos cuantos microsegundos así que el error podría hacerse muy grande en muy poco tiempo 

IntState = IntState + Error / FHz;    

ultimoError = Error;

IntState = constrain(IntState, -5.0f, 5.0f);

IntState es la acción Integral, gracias a la línea ultimoError = Error; el error se irá acumulando en cada bucle, imaginemos que tenemos un error de 5, al siguiente bucle ultimoError será 5 y se sumará el nuevo error, a continuación esa suma     se convertirá en  ultimoError y así seguirá aumentando hasta llegar al setpoint, por último para evitar que se genere un desbordamiento en la suma de errores utilizaremos la función constrain para limitar este valor IntState = constrain(IntState, -5.0f, 5.0f);

Derivativo: La acción Derivativa se encarga de reducir el error lo más rapido posible anticipando el siguiente error, de modo que funciona como un vistazo a futuro que permite disminuir la perturbación lo más rapido posible, en el programa usaremos el valor del acelerómetro para anticiparnos a la perturbación 

Kd_fb = -K_d * gyroRate;

gyroRate se calcula en el filtro de kalman.

De esta forma la suma de todas las acciones (Proporcional, Integral y Derivativa) queda de la siguiente manera:

 Kp_fb = K_p * Error;

    

    Ki_fb = K_i * IntState;

    

    Kd_fb = -K_d * gyroRate;

    TorqueCMD = Kp_fb + Ki_fb + Kd_fb;

Kp_fb, Kp_fb y Kp_fb son los valores finales de las tres acciones, K_p, K_i y K_d son los valores que habremos de afinar nosotros, cada valor será distinto y dependen del peso y forma de su robot, otro valor a ajustar es el delay del bucle, aunque este valor no pertenece necesariamente al control PID afecta considerablemente la estabilidad del mismo, al final del código está escrito 
delay(delaytime); la variable delaytime será la que tendremos que ajustar además, incluso aunque hicieran uno robot fisicamente identico al mio, estos valores podrían requerir un valor distinto, para ajustar dichos valores:

Comenzar con K_p, K_i, K_d igualando 0, delaytime en 4.00,  y trabajar con K_p primero. Prueba establecer K_p a un valor de 1 y observar el robot. El objetivo es conseguir que el robot se mantenga en vertical, incluso si es muy inestable. Si el robot llega más allá y cae, reducir el valor de K_p. Si la respuesta del robot parece lenta, aumente el valor Kp.

Una vez que el robot es capaz de mantenerse un tiempo aunque sea breve en vertical, asignar un valor de 0.01 a K_d . Intentar aumentar este valor hasta que vea menos oscilaciones.

Una vez que el robot sea más estable, asignar un valor de 10 a K_i. Si el valor de K_i es demasiado alta, el robot se sacudirá izquierda y derecha rápidamente. Si es demasiado baja, no se verá ninguna diferencia perceptible. El Integral es acumulativo por lo tanto  el valor K_i tiene un impacto significativo.
Una vez que el robot sea lo suficientemente estable, se puede aumentar o disminuir un poco el valor delaytime  y ver si todavía es suficiente o más estable. El valor delaytime afecta la estabilidad y es un factor importante a considerar.

El resultado final del algoritmo quedará en la variable TorqueCMD = Kp_fb + Ki_fb + Kd_fb;
para posteriormente transformarla en una señal PWM, la salida se debe limitar a 255 o menos  (en mi caso fue limitada a 200) ya que la salida máxima PWM es de 255.


///////////////////////////PID////////////////////////////////////////
    ultimoError = Error;
    Error = SetpointA - AnguloA;
    IntState = IntState + Error / FHz;
    IntState = constrain(IntState, -5.0f, 5.0f);

    Kp_fb = K_p * Error;
    
    Ki_fb = K_i * IntState;
    
    Kd_fb = -K_d * gyroRate;

    TorqueCMD = Kp_fb + Ki_fb + Kd_fb;
    TorqueCMD = constrain(TorqueCMD, -200, 200);