CONTROLADOR PID
Dependencies: DebounceIn DebouncedIn QEI TextLCD mbed
Diff: main.cpp
- Revision:
- 0:efe7bab86201
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Nov 30 01:04:42 2017 +0000 @@ -0,0 +1,410 @@ +#include "mbed.h" +#include "TextLCD.h" +#include "DebouncedIn.h" +#include "QEI.h" + +//Declaracion de entradas y salidas +DebouncedIn Boton1(PTE20); //Boton para confirmar +DebouncedIn Boton2(PTE21); //Boton para confirmar +AnalogOut accion_u(PTE30); +AnalogIn vcond(PTB0); + +//Configuracion encoder +QEI wheel (PTD5, PTD0, NC, 100); + +//Configuracion pantalla LCD +TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7 + +int Kp=0,Ki=0,Kd=0,read=0,var,Kc=0,Tc,readc=0,cont_mayor=0; +float Sp=0; +float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.01; //Ts Tiempo Muestreo +float yT,uk=0,ukm1=0,uk_inicial,uk_mayor,delta_uk,delta_uk_mayor,uc_mayor; +Timer timer; +Timer tc; + + +int main() +{ + + + lcd.cls(); + lcd.locate(1,0); + lcd.printf("Kp=%d",Kp); + lcd.locate(9,0); + lcd.printf("Ki%d=",Ki); + lcd.locate(1,1); + lcd.printf("Kd=%d",Kd); + lcd.locate(9,1); + lcd.printf("Sp=%.1f",Sp); + + +set_Kp: + lcd.cls(); + lcd.locate(0,0); + lcd.printf(">Kp= "); + + lcd.locate(9,0); + lcd.printf("Ki=%d",Ki); + lcd.locate(1,1); + lcd.printf("Kd=%d",Kd); + lcd.locate(9,1); + lcd.printf("Sp=%.1f",Sp); + + + while(1) { + + Kp=Kp+wheel.getPulses(); + wheel.reset(); + + if(Kp>=999) { + Kp=999; + } else if (Kp<=0) { + Kp=0; + } + + lcd.locate(0,0); + lcd.printf(">Kp=%d ",Kp); + + if(Boton2.falling()) { + goto set_Ki; + } + + + if(Boton1.falling()) { + goto PID; + } + + } + +set_Ki: + + lcd.cls(); + lcd.locate(8,0); + lcd.printf(">Ki= "); + + lcd.locate(1,0); + lcd.printf("Kp=%d",Kp); + lcd.locate(1,1); + lcd.printf("Kd=%d",Kd); + lcd.locate(9,1); + lcd.printf("Sp=%.1f",Sp); + + //Ki=0; + while(1) { + + Ki=Ki+wheel.getPulses(); + wheel.reset(); + + if(Ki>=999) { + Ki=999; + } else if (Ki<=0) { + Ki=0; + } + + lcd.locate(8,0); + lcd.printf(">Ki=%d ",Ki); + + if(Boton2.falling()) { + goto set_Kd; + } + + if(Boton1.falling()) { + goto PID; + } + + } + +set_Kd: + + lcd.cls(); + lcd.locate(0,1); + lcd.printf(">Kd= "); + + lcd.locate(1,0); + lcd.printf("Kp=%d",Kp); + lcd.locate(9,0); + lcd.printf("Ki=%d",Ki); + lcd.locate(9,1); + lcd.printf("Sp=%.1f",Sp); + + + + while(1) { + + Kd=Kd+wheel.getPulses(); + wheel.reset(); + + if(Kd>=999) { + Kd=999; + } else if (Kd<=0) { + Kd=0; + } + + lcd.locate(0,1); + lcd.printf(">Kd=%d ",Kd); + + if(Boton2.falling()) { + goto set_Sp; + } + + if(Boton1.falling()) { + goto PID; + } + + } + +set_Sp: + + lcd.cls(); + lcd.locate(8,1); + lcd.printf(">Sp= "); + + lcd.locate(1,0); + lcd.printf("Kp=%d",Kp); + lcd.locate(9,0); + lcd.printf("Ki=%d",Ki); + lcd.locate(1,1); + lcd.printf("Kd=%d",Kd); + + //Sp=0; + while(1) { + + Sp=Sp+0.1*wheel.getPulses(); + wheel.reset(); + + if(Sp>=3.3) { + Sp=3.3; + } else if (Sp<=0) { + Sp=0; + } + + lcd.locate(8,1); + lcd.printf(">Sp=%.1f ",Sp); + + if(Boton2.falling()) { + goto PID2; + } + + if(Boton1.falling()) { + goto PID; + } + } + + + +PID2: + lcd.cls(); + Kp = 1; + +while (Kp<100) { + + read = timer.read(); + + if (read==0) { + timer.start(); + uk_mayor=0; + } + + if (read==1) { + uk_inicial=uk; + } + + if (read==5) { + if (Kp<100) { + Kp = Kp+10; + } + timer.reset(); + } + + if (uk>uk_mayor) { + uk_mayor=uk; + } + + if (read==4) { + delta_uk=uk_mayor-uk_inicial; + if (delta_uk>delta_uk_mayor) { + delta_uk_mayor=delta_uk; + Kc=Kp; + //Tc=2; + uc_mayor=uk_mayor; + + } + } + + if (Kp>100) { + goto CALCULO_TC; + } + + + + yT=vcond.read()*3.3; + ek=Sp-yT; + + + K1=Kp+(Ki/2)*Ts+(Kd/Ts); + K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts); + K3=Kd/Ts; + + uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control + + if (uk>3.3) { //Salida PID si es mayor que el MAX + uk=3.3; + } else if (uk<0) { //Salida PID si es menor que el MIN + uk=0; + } + + //control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta. + accion_u = uk; + + if (uk>uk_mayor) { + uk_mayor=uk; + } + + ekm2=ekm1; + ekm1=ek; + ukm1=uk; + lcd.locate(0,0); + //lcd.printf("Error=%.2f ",ek); + lcd.printf("a_tunning"); + + } + + + +PID: + lcd.cls(); + while(1){ + + yT=vcond.read()*3.3; + ek=Sp-yT; + + //Conversion de parametros PID Continuo a parametros de PID Digital + //ESTO ES UN PID DIGITAL Y LOS PARAMETROS AGREGADOS SON PARA UN PID CONTINUO!! + K1=Kp+(Ki/2)*Ts+(Kd/Ts); + K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts); + K3=Kd/Ts; + + uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control + + if (uk>3.3) { //Salida PID si es mayor que el MAX + uk=3.3;} + else if (uk<0){ //Salida PID si es menor que el MIN + uk=0; + } + + //control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta. + accion_u = uk; + + ekm2=ekm1; + ekm1=ek; + ukm1=uk; + lcd.locate(0,0); + lcd.printf("Error=%.2f ",ek); + lcd.locate(0,1); + lcd.printf("Y_act=%.2f",yT); + wait(Ts); // Muestreos + } + + + +CALCULO_TC: + while(readc<21) { + + Kp=Kc; + tc.start(); + readc=tc.read(); + + if (uk==uc_mayor) { + cont_mayor= cont_mayor+1; + } + + if (readc==20) { + Tc=20/cont_mayor; + goto CONTROL_AT; + } + + yT=vcond.read()*3.3; + ek=Sp-yT; + + + K1=Kp+(Ki/2)*Ts+(Kd/Ts); + K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts); + K3=Kd/Ts; + + uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control + + if (uk>3.3) { //Salida PID si es mayor que el MAX + uk=3.3; + } else if (uk<0) { //Salida PID si es menor que el MIN + uk=0; + } + + //control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta. + accion_u = uk; + + if (uk>uk_mayor) { + uk_mayor=uk; + } + + ekm2=ekm1; + ekm1=ek; + ukm1=uk; + lcd.locate(0,0); + + lcd.printf("a_tunnig"); + + + } + +CONTROL_AT: + + while(1) { + Kp=0.45*Kc; + Ki=0.54*Kc/Tc; + Kd=0; + + yT=vcond.read()*3.3; + ek=Sp-yT; + + + K1=Kp+(Ki/2)*Ts+(Kd/Ts); + K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts); + K3=Kd/Ts; + + uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control + + if (uk>3.3) { //Salida PID si es mayor que el MAX + uk=3.3; + } else if (uk<0) { //Salida PID si es menor que el MIN + uk=0; + } + + //control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta. + accion_u = uk; + + if (uk>uk_mayor) { + uk_mayor=uk; + } + + ekm2=ekm1; + ekm1=ek; + ukm1=uk; + //lcd.locate(0,0); + //lcd.printf("Error=%.2f ",ek); + //lcd.printf("delta_mayor=%.2f",delta_uk_mayor); + //lcd.locate(0,1); + //lcd.printf("Kc=%I",Kc); + lcd.locate(0,0); + lcd.printf("Error=%.2f ",ek); + lcd.locate(0,1); + lcd.printf("Y_act=%.2f",yT); + wait(Ts); // Muestreos + lcd.locate(13,1); + lcd.printf("%.I",Kp); + wait(Ts); // Muestreos + } + +} + + +