Controlador PID Genérico que se parametriza por medio de un encoder rotatorio- Sp (Set-point), Kp(Ganancia proporcional) ,Ki(Constante integral), Kd(Constante Derivativa), usa dos pulsadores adicionales
Dependencies: QEI TextLCD mbed
Este programa es un control PID genérico que se parametriza usando un encoder manual. Se puede ajustar. Set-Point Kp Ki Kd
Emplea el pulsador central del encoder para cambiar de parámetro y otro adicional para consolidar los parámetros y ejecutar el controlador
Se puede probar por medio de un sistema de primer orden conformado por una red RC. de 11K y un condensador de 22uF desde la salida hasta la entrada. que se ve en el mini protoboard de la figura siguiente
a continuación mostramos la respuesta del controlador PID a esos parámetros trabajando en lazo cerrado. que da cero error
Donde se ve, Error (Er), Medida(Me), Setpoint(Sp), Control o salida(Co) de voltaje.
Se aprecia la izquierda el encoder manual y en la entrada análoga se aplicó un anillo de ferrita o balum; ya que estas entradas análogas son muy sensibles al ruido.
Abajo se aprecia el esquemático. Las conexiones del LCD se deducen de los listados de programa y de la guia del CookBook de esta webb, en la parte de LCD.
Si quiere ver la respuesta del sistema a cambios de carga, coloque de forma temporal una resistencia de 100K en paralelo con el condensador de 22uF.
Bug del esquemático (intercambie los puertos de los dos pulsadores)
main.cpp
- Committer:
- tony63
- Date:
- 2016-04-22
- Revision:
- 0:4e0dfcf0e7ce
- Child:
- 1:058b8f5c135d
File content as of revision 0:4e0dfcf0e7ce:
#include "mbed.h" #include "QEI.h" #include "TextLCD.h" TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 QEI encoder (PTA13, PTD5, NC, 624); AnalogIn y(PTB0);//entrada analoga AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer //si se ignora esto se arruina la FRDMKL25Z DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalIn button3(PTC16);//cambia ingreso de los 4 parametros DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop //codigos movimiento del curzor //int C1=0x0E; // solo muestra el curzor int C2=0x18; // desplaza izquierda int C3=0x1A; // desplaza derecha int C4=0x0C; // quito cursor bajo int C1=0x0F; int err, med, yr, pid, ap, ai, ad, err_v, cambio=0, diferencia=0; float pidn; int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1; int flagt=0; Timer t; int main() { lcd.locate(0,1); lcd.printf("**Control PID**"); wait(2); lcd.cls(); // Borrar Pantalla lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD lcd.locate(8,0); lcd.printf("Kp=%d",kpnum); lcd.locate(0,1); lcd.printf("Ki=%d",kinum); lcd.locate(8,1); lcd.printf("Kd=%d",kdnum); lcd.locate(0,0); lcd.printf("Sp=%d",spnum); while(1) { //lcd.locate(8,0); //lcd.printf("Kp=%d",encoder.getPulses()); //wait(.5); diferencia=encoder.getPulses()-cambio; cambio=encoder.getPulses(); if (diferencia==0) { //nada } else if(diferencia>0) { if(pos==1) { if(spnum+diferencia>=9999) { spnum=9999; lcd.locate(3,0); lcd.printf(" "); lcd.locate(3,0); lcd.printf("%d", spnum); } else { spnum+=diferencia; lcd.locate(3,0); lcd.printf("%d", spnum); } } else if(pos==2) { if(kpnum+diferencia>=9999) { kpnum=9999; lcd.locate(11,0); lcd.printf(" "); lcd.locate(11,0); lcd.printf("%d", kpnum); } else { kpnum+=diferencia; lcd.locate(11,0); lcd.printf("%d", kpnum); } } else if(pos==3) { if(kinum+diferencia>=9999) { kinum=9999; lcd.locate(3,1); lcd.printf(" "); lcd.locate(3,1); lcd.printf("%d", kinum); } else { kinum+=diferencia; lcd.locate(3,1); lcd.printf("%d", kinum); } } else if(pos==4) { if(kdnum+diferencia>=9999) { kdnum=9999; lcd.locate(11,1); lcd.printf(" "); lcd.locate(11,1); lcd.printf("%d", kdnum); } else { kdnum+=diferencia; lcd.locate(11,1); lcd.printf("%d", kdnum); } } } else if(diferencia<0) { if(pos==1) { if(spnum+diferencia<0) { //No ocurre nada } else { spnum+=diferencia; lcd.locate(3,0); lcd.printf(" "); lcd.locate(3,0); lcd.printf("%d", spnum); } } else if(pos==2) { if(kpnum+diferencia<0) { //No ocurre nada } else { kpnum+=diferencia; lcd.locate(11,0); lcd.printf(" "); lcd.locate(11,0); lcd.printf("%d", kpnum); } } else if(pos==3) { if(kinum+diferencia<0) { //No ocurre nada } else { kinum+=diferencia; lcd.locate(3,1); lcd.printf(" "); lcd.locate(3,1); lcd.printf("%d", kinum); } } else if(pos==4) { if(kdnum+diferencia<0) { //No ocurre nada } else { kdnum+=diferencia; lcd.locate(11,1); lcd.printf(" "); lcd.locate(11,1); lcd.printf("%d", kdnum); } } } if (!button3) //cambia la posicion de ingreso de parametros { led3 =!led3; if(pos==4) { pos=1; lcd.locate(3,0); lcd.printf("%d", spnum); } else if (pos==1) { pos++; lcd.locate(11,0); lcd.printf("%d", kpnum); } else if(pos==2) { pos++; lcd.locate(3,1); lcd.printf("%d", kinum); } else if(pos==3) { pos++; lcd.locate(11,1); lcd.printf("%d", kdnum); } wait(0.25); } if (!button4) { break; //sale del bucle si pisan suiche4 } wait(0.1); } //Transicion lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo lcd.cls(); //borra la pantalla lcd.printf(" GUARDADOS!"); wait(1); lcd.cls(); lcd.printf(" INICIA EL PID"); wait(1); // se imprimen los parches del control ***************************************** lcd.cls(); lcd.printf("Er=%d",err); lcd.locate(8,0); lcd.printf("Me=%d",med); lcd.locate(0,1); lcd.printf("Sp=%d",spnum); lcd.locate(8,1); lcd.printf("Co=%d",pid); wait(2); // CICLO PRINCIPAL CONTROLADOR PID flagt=0; while(1) { med=999*y.read(); //leer puerto analogo y asignar a med err = (spnum-med); //se calcula el error ap = kpnum*err; //se calcula la accion proporcinal // se verifica que la accion integral no sea muy grande if(ai<100) { ai =(kinum*err)+ai; //calculo de la integral del error } ad = kdnum*(err-err_v); //calculo de la accion derivativa pid = (ap+ai+ad); // se verifica que pid sea positivo ************************************** if(pid<=0) { pid=0; } // se verifica que pid sea menor o igual la valor maximo ***************** if (pid > 999) { pid=999; } // se actualizan las variables ******************************************* err_v = err; //se muestran las variables****************************************** if(flagt==0) { t.start(); flagt=1; } if(t>=0.3) { lcd.locate(3,0); lcd.printf(" "); lcd.locate(3,0); lcd.printf("%d",err); lcd.locate(11,0); lcd.printf(" "); lcd.locate(11,0); lcd.printf("%d",med); lcd.locate(3,1); lcd.printf(" "); lcd.locate(3,1); lcd.printf("%d",spnum); lcd.locate(11,1); lcd.printf(" "); lcd.locate(11,1); lcd.printf("%d",pid); flagt=0; t.reset(); } //Normalizacion de la salida pidn=pid/999; // se envia el valor pid a puerto analogico de salida (D/A) ************** u.write(pidn); // se repite el ciclo wait(0.005); } }