main.cpp

00001 // Hello World! for the TextLCD
00002  
00003 #include "mbed.h"
00004 #include "stdio.h"
00005 #include "TextLCD.h"
00006 #include "DebouncedIn.h" 
00007 #include "QEI.h"
00008 
00009 //Declaracion de entradas y salidas
00010 DebouncedIn Boton1(PTE20);  //Boton para confirmar
00011 DebouncedIn Boton2(PTE21);  //Boton para confirmar
00012 
00013 //Configuracion encoder
00014 QEI wheel (PTD5, PTD0, NC, 100);
00015 
00016 PwmOut control(PTE29);
00017 AnalogIn vcond(PTB0);
00018 
00019 //Configuracion pantalla LCD 
00020 TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7
00021 
00022 //Variables y parametros del control PID
00023 int Kp=0,Ki=0,Kd=0;
00024 float Sp=0;
00025 float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.1; //Ts Tiempo Muestreo
00026 float yT,uk=0,ukm1=0;
00027 
00028 
00029 
00030 int main() {
00031     
00032     lcd.cls();
00033     lcd.locate(1,0);
00034     lcd.printf("Kp=%d",Kp);
00035     lcd.locate(9,0);
00036     lcd.printf("Ki%d=",Ki);
00037     lcd.locate(1,1);
00038     lcd.printf("Kd=%d",Kd);
00039     lcd.locate(9,1);
00040     lcd.printf("Sp=%.1f",Sp);
00041     
00042     //Configuracion de constante Kp
00043     set_Kp:
00044     lcd.cls();
00045     lcd.locate(0,0);
00046     lcd.printf(">Kp=   ");
00047     
00048     lcd.locate(9,0);
00049     lcd.printf("Ki=%d",Ki);
00050     lcd.locate(1,1);
00051     lcd.printf("Kd=%d",Kd);
00052     lcd.locate(9,1);
00053     lcd.printf("Sp=%.1f",Sp);
00054     
00055     
00056     while(1){
00057          
00058     Kp=Kp+wheel.getPulses();
00059     wheel.reset();
00060               
00061      if(Kp>=999){
00062             Kp=999;
00063      }
00064      else if (Kp<=0){
00065              Kp=0;
00066      }
00067      
00068     lcd.locate(0,0);
00069     lcd.printf(">Kp=%d  ",Kp);
00070     
00071     if(Boton2.falling()){
00072         goto set_Ki;
00073         }
00074         
00075         
00076     if(Boton1.falling()){
00077         goto PID;
00078     }
00079         
00080     }
00081     
00082     //Configuracion de Ki
00083     set_Ki:
00084     
00085     lcd.cls();
00086     lcd.locate(8,0);
00087     lcd.printf(">Ki=   ");
00088     
00089     lcd.locate(1,0);
00090     lcd.printf("Kp=%d",Kp);
00091     lcd.locate(1,1);
00092     lcd.printf("Kd=%d",Kd);
00093     lcd.locate(9,1);
00094     lcd.printf("Sp=%.1f",Sp);
00095     
00096     while(1){
00097          
00098      Ki=Ki+wheel.getPulses();
00099      wheel.reset();
00100      
00101      if(Ki>=999){
00102             Ki=999;
00103      }
00104      else if (Ki<=0){
00105              Ki=0;
00106      }
00107      
00108     lcd.locate(8,0);
00109     lcd.printf(">Ki=%d  ",Ki);
00110     
00111     if(Boton2.falling()){
00112         goto set_Kd;
00113         }
00114         
00115     if(Boton1.falling()){
00116         goto PID;
00117     }
00118         
00119     }
00120     
00121     //Configuracion de Kd
00122     set_Kd:
00123     
00124     lcd.cls();
00125     lcd.locate(0,1);
00126     lcd.printf(">Kd=   ");
00127     
00128     lcd.locate(1,0);
00129     lcd.printf("Kp=%d",Kp);
00130     lcd.locate(9,0);
00131     lcd.printf("Ki=%d",Ki);
00132     lcd.locate(9,1);
00133     lcd.printf("Sp=%.1f",Sp);
00134     
00135     
00136     
00137     while(1){
00138          
00139      Kd=Kd+wheel.getPulses();
00140      wheel.reset();
00141      
00142      if(Kd>=999){
00143             Kd=999;
00144      }
00145      else if (Kd<=0){
00146              Kd=0;
00147      }
00148      
00149     lcd.locate(0,1);
00150     lcd.printf(">Kd=%d  ",Kd);
00151     
00152     if(Boton2.falling()){
00153         goto set_Sp;
00154     }
00155     
00156     if(Boton1.falling()){
00157         goto PID;
00158     }
00159         
00160     }
00161     
00162     //Configuracion de Sp
00163     set_Sp:
00164     
00165     lcd.cls();
00166     lcd.locate(8,1);
00167     lcd.printf(">Sp=   ");
00168     
00169     lcd.locate(1,0);
00170     lcd.printf("Kp=%d",Kp);
00171     lcd.locate(9,0);
00172     lcd.printf("Ki=%d",Ki);
00173     lcd.locate(1,1);
00174     lcd.printf("Kd=%d",Kd);
00175             
00176    
00177     while(1){
00178          
00179      Sp=Sp+0.1*wheel.getPulses();
00180      wheel.reset();
00181      
00182      if(Sp>=3.3){
00183             Sp=3.3;
00184      }
00185      else if (Sp<=0){
00186              Sp=0;
00187      }
00188      
00189     lcd.locate(8,1);
00190     lcd.printf(">Sp=%.1f  ",Sp);
00191     
00192     if(Boton2.falling()){
00193         goto set_Kp;
00194     }
00195     
00196     if(Boton1.falling()){
00197         goto PID;
00198     }
00199                
00200     }
00201     
00202     //Inicia algoritmo del PID
00203     PID:
00204     
00205     lcd.cls();
00206     while(1){
00207     
00208     yT=vcond.read()*3.3;
00209     ek=Sp-yT;
00210     
00211     //Conversion de parametros PID Continuo a parametros de PID Digital 
00212     //ESTO ES UN PID DIGITAL Y LOS PARAMETROS AGREGADOS SON PARA UN PID CONTINUO!!
00213     
00214     K1=Kp+(Ki/2)*Ts+(Kd/Ts);
00215     K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
00216     K3=Kd/Ts;
00217     
00218     uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control
00219     
00220     if (uk>3.3) {        //Salida PID si es mayor que el MAX 
00221         uk=3.3;}                          
00222      else if (uk<0){      //Salida PID si es menor que el MIN 
00223         uk=0;                         
00224         }
00225                  
00226         control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta.
00227         
00228         //Actualizacion de datos para el siguiente paso
00229         ekm2=ekm1; 
00230         ekm1=ek;
00231         ukm1=uk;
00232         
00233         //Mostrando error y salida actual
00234         lcd.locate(0,0);
00235         lcd.printf("Error=%.2f ",ek);
00236         lcd.locate(0,1);
00237         lcd.printf("Y_act=%.2f",yT);
00238         wait(Ts); //Tiempo de muestreo
00239     }
00240     
00241 }
00242 
00243