Función PID

► función control PID

Para utilizar el control PID en nuestro robot seguidor de líneas necesitamos tener listas y testeadas las funciones para control de motores, lectura de sensores con calculo de error, indicaciones luminosos y el pulsador de arranque.

El control PID se utiliza básicamente para calcular las correcciones en la velocidad y sentido de giro de los motores, con el objetivo de eliminar el error (perder la posición de la linea).

La velocidad máxima a la que queremos que nuestro robot funcione estará establecida por nosotros, solo se vera alterada cuando el robot no este en el centro de la linea.

Si estamos perdiendo la linea hacia la derecha y aplicamos un ajuste de velocidad en los motores para retomar el centro de manera brusca, empezaremos a perder la linea por la izquierda, entraríamos en un cabeceo de la trompa del robot tratando de recuperar el centro que solo nos pondría mas lentos y con riesgo de perder la linea completamente.

Aquí el control PID cumple la función de atenuar y anticipar los ajustes sin que sean sean bruscos para que el robot pueda seguir la linea lo mas estable posible sin oscilaciones ni cabeceos.

Es importante antes comprender el funcionamiento del PID y cada una de sus componentes para poder hacer el ajuste.

► Orden de acciones en el código del robot en general.

Básicamente el orden de acciones son:

Lectura de sensores.
Calculo de Error.
Control PID.
Ajuste de Velocidad y sentido de giro a los motores.

Esto se repetirá en bucle mientras el robot esté en funcionamiento.

► Incorporar control PID a nuestro código.

Parámetros y variables que debemos colocar en código para el control PID.

☼ Incorporamos las líneas de código para las variables que cuyos valores establecemos nosotros por medio de las pruebas del robot:

Velocidad de Robot (vel_set).
Constate KP.
Constante KD.
Constante KI.

Líneas a insertar antes del setup:

int vel_set = 0;

float KP = 10;

float KD = 1.5;

float KI = 0;

☼ Luego las líneas de código para las variables que corresponden al los cálculos del PID:

izq - velocidad motor izquierdo.
der - velocidad motor derecho.
vel - velocidad de ajuste calculada por el PID.
P - Posición relativa (error)
D - Derivada
I - Integral
EA - Error anterior (o sumatoria de errores anteriores)

/* VARIABLES CONTROL */

int izq = 0;

int der = 0;

int vel = 0;

int P = 0;

int D = 0;

int I = 0;

int EA = 0;

☼ Detalles de la función control:

El la función de control tenemos que:

Igualar la variable P al error calculado en la función promedio ponderado.
Colocar las formulas de la derivada y la integral.
Formula para el calculo PID de la velocidad de ajuste (vel).
Establecer la fórmalas para el ajuste de velocidad izq y der, restando o sumando la vel calculada por el control PID a la vel_set (debemos realizar un testeo del funcionamiento del código para comprobar que el ajuste es correcto, en caso contrario invertimos suma por resta y viceversa)
Vamos a establecer un limite en las velocidades de control izq y der para los dos signos (positivo y negativo) que no supere el limite del PWM (255).
Al final de la función vamos a definir que el error anterior EA sea igual a la posición actual P y así se ajustará en cada bucle.

► Función control.

void control() {

P = Error; // posición relativa

D = P - EA; // calculo derivado

I = I + EA; // calculo integral

/*Limite de integral*/

if (I >= 1000) {

I = 1000;

}

if (I <= -1000) {

I = -1000;

}

vel = (P*KP) + (D*KD) + (I*KI); // calculo vel ajuste con PID

izq = vel_set - vel; // ajuste de velocidad izq

der = vel_set + vel; // ajuste de velocidad der

/*Limite de velocidad a max PWM*/

if (izq >= 255) {

izq = 255;

}

if (izq <= -255) {

izq = -255;

}

if (der >= 255) {

der = 255;

}

if (der <= -255) {

der = -255;

}

EA = P; // Error anterior igual al actual

}

☼ Una vez que llegamos aquí con nuestro robot ya armado, uniremos las partes de código y funciones con los parámetros de nuestro robot y realizamos los testeos de funcionamiento básicos con la vel_set en 0 para una mejor apreciación de:

Led indicadores.
Pulsador de arranque.
Control PID (sentido de giro de ajuste de los motores).

► Testeo del código completo en robot MECHA 22.

Resultados del Test:

Los indicadores funcionan correctamente.

▬ El led azul encendido indica que esta listo para largar.

▬ Los verdes en la nariz indican el modo RUN.

El pulsador de arranque funciona correctamente

▬ Al pulsar el pulsador se apaga el led azul indicando que cuando soltemos el pulsador se ejecuta el bucle infinito del modo RUN.

☼ La comprobación del control PID de ajuste de velocidades izq y der no es correcto.

▬ Cuando movemos la nariz hacia la derecha y la izquierda los motores ajustan a favor del movimiento lo que nos dejaría fuera de la linea, lo solucionamos cambiando los signos en el ajuste actual:

izq = vel_set - vel; // ajuste de velocidad izq

der = vel_set + vel; // ajuste de velocidad der

Invirtiendo los signos de suma y resta:

izq = vel_set + vel; // ajuste de velocidad izq

der = vel_set - vel; // ajuste de velocidad der

☼ Comprobación final:

Para la comprobación final vamos a dejar la vel_set en 0 y colocaremos la batería para que los motores tengan el torque nominal.

Colocamos el robot en el centro de la linea y si intentamos correrlo este debería volver a centrarse sin avanzar ya que no tiene velocidad de marcha (vel_set = 0)

Testeo Final Robot SL

☼ Ya que tenemos nuestro robot testeado podemos ahora realizar pruebas en la pista donde ajustaremos vel_set de a poco y las constantes del PID (KP,KD,KI) para lograr el mejor funcionamiento de nuestro robot.

☼ El comportamiento del robot será diferente si la pista y las condiciones no son las mismas, pero con mucha practica aprenderemos el comportamiento del robot para poder ajustar los parámetros rápidamente.

▼ Código de testeo base completo ▼

/*Código base*/

/*Robot MECHA 22*/

/*ALOSI BOT - Isola Gustavo*/

/*Seteo de parametros de control*/

int vel_set = 0; // velocidad PWM

float KP = 2.5;

float KD = 2.5;

float KI = 0.01;

/*etiquetas de pines*/

/*pulsador*/

#define pul 2

/*indicadores led*/

#define led1 8

#define led2 10

#define led_A 13

/*driver*/

#define pwmi 3

#define mot_izq_A1 5

#define mot_izq_A2 4

#define pwmd 9

#define mot_der_B1 7

#define mot_der_B2 6

/*sensores*/

#define S1 A7

#define S2 A6

#define S3 A5

#define S4 A4

#define S5 A3

#define S6 A2

#define S7 A1

#define S8 A0

/*creacion de varibles*/

/*lectura de sensores*/

int LS1 = 0;

int LS2 = 0;

int LS3 = 0;

int LS4 = 0;

int LS5 = 0;

int LS6 = 0;

int LS7 = 0;

int LS8 = 0;

/* Variable poscicion relativa*/

float ponderacion = 0;

float suma_lecturas = 0;

float pos_rel = 0;

int Error = 0;

/* VARIABLES CONTROL */

int izq = 0;

int der = 0;

int vel = 0;

int P = 0;

int D = 0;

int I = 0;

int EA = 0;

void setup() {

/*configuracion de pines*/

pinMode(pul, INPUT);

pinMode(led_A,OUTPUT);

pinMode(led1,OUTPUT);

pinMode(led2,OUTPUT);

pinMode(mot_izq_A1,OUTPUT);

pinMode(mot_izq_A2,OUTPUT);

pinMode(mot_der_B1,OUTPUT);

Serial.begin(9600); // incio monitor serial

}

void loop() {

digitalWrite(led_A,HIGH);

while (digitalRead(pul)); // pulsador sin pulsar el código se detiene aquí

while (digitalRead(pul)==0){ // pulsador pulsado esperando para largar

digitalWrite(led_A,LOW);

}

digitalWrite(led1,HIGH);

digitalWrite(led2,HIGH);

while (true) { // bucle infinito inicio

promedio_ponderado();

control();

motores(izq,der); // marcha del robot

}

void motores (int VmIzq, int VmDer) {

/*Dirección Motor Izquierdo*/

if (VmIzq >= 0) { //si la velocidad calculada es mayor o igual a 0, el giro es adelante

digitalWrite(mot_izq_A1,HIGH); //colocamos los estados para sentido de giro adelante en el motor izq.

digitalWrite(mot_izq_A2,LOW);

}

else { // si la velocidad no es mayor a 0, esta será negativa, giro atrás

digitalWrite(mot_izq_A1,LOW); // colocamos los estados para sentido de giro atrás en el motor izq.

digitalWrite(mot_izq_A2,HIGH);

VmIzq = VmIzq * -1; // la velocidad no puede ser negativa, así que le cambiamos el signo

} // por que el sentido de giro atrás ya fue establecido.

if (VmDer >= 0) { // si la velocidad calculada es mayor o igual a 0, el giro es adelante

digitalWrite(mot_der_B1,LOW); // colocamos los estados para sentido de giro adelante en el motor der.

digitalWrite(mot_der_B2,HIGH);

}

else { // si la velocidad no es mayor a 0, esta será negativa, giro atrás

digitalWrite(mot_der_B1,HIGH); // colocamos los estados para sentido de giro atrás en el motor der.

digitalWrite(mot_der_B2,LOW);

VmDer = VmDer * -1; // la velocidad no puede ser negativa, así que le cambiamos el signo

} // por que el sentido de giro atrás ya fue establecido.

analogWrite(pwmi,VmIzq); // escribimos la velocidad en la salida PWM del micro para motor izq.

analogWrite(pwmd,VmDer); // escribimos la velocidad en la salida PWM del micro para motor der.

} // fin control de motores

void promedio_ponderado() { // creamos la funcion vacia

// Lectura de sesnores //

LS1 = analogRead(S1); // sensor extremo izquierdo

LS2 = analogRead(S2);

LS3 = analogRead(S3);

LS4 = analogRead(S4);

LS5 = analogRead(S5);

LS6 = analogRead(S6);

LS7 = analogRead(S7);

LS8 = analogRead(S8); // sensor extremo derecho

/* Mapeo de lecturas */

LS1 = map(LS1, 0,1023,1000,0);

LS2 = map(LS2, 0,1023,1000,0);

LS3 = map(LS3, 0,1023,1000,0);

LS4 = map(LS4, 0,1023,1000,0);

LS5 = map(LS5, 0,1023,1000,0);

LS6 = map(LS6, 0,1023,1000,0);

LS7 = map(LS7, 0,1023,1000,0);

LS8 = map(LS8, 0,1023,1000,0);

/*calculos de posicion relativa*/

ponderacion = ((LS1*0)+(LS2*1)+(LS3*2)+(LS4*3)+(LS5*4)+(LS6*5)+(LS7*6)+(LS8*7));

suma_lecturas = (LS1+LS2+LS3+LS4+LS5+LS6+LS7+LS8);

pos_rel = 1000*(ponderacion/suma_lecturas);

Error = pos_rel - 3500;

} // fin promedio ponderado

void monitorserie() {

/* Muestra de valores en monitor serial */

Serial.print(" | ");

Serial.print(LS1); Serial.print(" | ");

Serial.print(LS2); Serial.print(" | ");

Serial.print(LS3); Serial.print(" | ");

Serial.print(LS4); Serial.print(" | ");

Serial.print(LS5); Serial.print(" | ");

Serial.print(LS6); Serial.print(" | ");

Serial.print(LS7); Serial.print(" | ");

Serial.print(LS8); Serial.println(" | ");

delay(500); // retardo de tiempo

/* Muestra de valores calculados posicion relativa*/

Serial.print("ponderacion = ");Serial.println(ponderacion);

Serial.print("suma = ");Serial.println(suma_lecturas);

Serial.print("pos_rel = ");Serial.println(pos_rel);

Serial.print("Error = ");Serial.println(Error);

Serial.println(" | ");

/*Para las pruebas del robot ya en pista se deben comentar las impresiones del monitor serial*/

} // fin monitorserie

void control() {

digitalWrite(led_A, LOW);

if(Error > 1350) {

digitalWrite(led_A, HIGH);

while(analogRead(A2)>700){

motores(-255,255);

}

if(Error < -1350) {