PID DISCRETO
Dependencies: mbed QEI TextLCD DebouncedIn
main.cpp
- Committer:
- demo71
- Date:
- 2015-10-27
- Revision:
- 1:f0fa90a6fb6e
- Parent:
- 0:694446246c35
- Child:
- 2:921bc4072c83
File content as of revision 1:f0fa90a6fb6e:
// Hello World! for the TextLCD #include "mbed.h" #include "stdio.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 //Configuracion encoder QEI wheel (PTD5, PTD0, NC, 100); PwmOut control(PTE29); AnalogIn vcond(PTB0); //Configuracion pantalla LCD TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7 //codigos movimiento del cursor LCD //int C2=0x18; // desplaza izquierda //int C3=0x1A; // desplaza derecha //int C4=0x0C; // quito cursor bajo //int C1=0x0F; // solo muestra el cursor int Kp=0,Ki=0,Kd=0; float Sp=0; float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.1; //Ts Tiempo Muestreo float yT,uk=0,ukm1=0; 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); //Kd=0; 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 set_Kp; } if(Boton1.falling()){ goto PID; } } 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. 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 } }