![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
controlador PID con encoder
Dependencies: DebouncedIn QEI TextLCD mbed
main.cpp
- Committer:
- szapataa
- Date:
- 2015-11-13
- Revision:
- 0:43669293f24e
File content as of revision 0:43669293f24e:
// PID con encoder y boton #include "mbed.h" #include "stdio.h" #include "TextLCD.h" #include "DebouncedIn.h" #include "QEI.h" //Declaracion de entradas y salidas DebouncedIn Boton1(PTA1); //Boton para confirmar DebouncedIn Boton2(PTC9); //Boton para pasar //Configuracion encoder QEI wheel (PTD5, PTD0, NC, 100); PwmOut control(PTE29); AnalogIn vsal(PTB0); 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,Sp=0,p,i; float rT,eT,iT,dT,yT,uT,iT0=0,eT0=0,iT_1=0,eT_1=0,spf=0; Timer tu; Timer td; int main() { lcd.cls(); lcd.locate(0,0); lcd.printf("PID_ENCODER"); wait(1); lcd.cls(); lcd.locate(0,0); lcd.printf("Omar Torres"); lcd.locate(0,1); lcd.printf("Sebastian Zapata"); wait(2); lcd.cls(); //comando basado en el manual del LCD 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=%d",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=%d",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=%d",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=%d",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+wheel.getPulses(); wheel.reset(); if(Sp>=330){ Sp=330; } else if (Sp<=0){ Sp=0; } lcd.locate(8,1); lcd.printf("|Sp=%d ",Sp); if(Boton2.falling()){ goto set_Kp; } if(Boton1.falling()){ goto PID; } } PID: lcd.cls(); lcd.locate(0,0); lcd.printf("Guardando.."); wait(0.6); lcd.cls(); lcd.locate(0,0); lcd.printf("Guardando...."); wait(0.6); lcd.cls(); lcd.locate(0,0); lcd.printf("Guardando......"); wait(0.6); lcd.cls(); spf=(float)Sp/100; //convirtiendo de decivoltios a voltios while(1){ yT=vsal.read()*3.3; eT=spf-yT; iT=Ki*eT+iT0; //Accion Integral dT=Kd*(eT-eT0); //Accion Derivativa uT=iT+Kp*eT+dT; if (uT>3.3) { //Salida PID si es mayor que el MAX uT=3.3;} else if (uT<0){ //Salida PID si es menor que el MIN uT=0; } iT0=iT; //Guarda las variables eT0=eT; control=(float)uT/3.3; lcd.locate(0,0); lcd.printf("Err=%.2f ",eT); lcd.locate(0,1); lcd.printf("Sal=%.2f",yT); wait(0.2); } }