main.cpp

00001 #include "mbed.h"
00002 #include "TextLCD.h"
00003 #include "DebouncedIn.h"
00004 #include "QEI.h"
00005 
00006 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5);  //configura los puestos PTE0,PTE1,PTE2,PTE3,PTE4,PTE5, donde se conectara el LCD 16x2
00007 QEI conteo(PTA13,PTD5,NC,12);
00008 DebouncedIn salto(PTC16);
00009 DebouncedIn set(PTC17);
00010 AnalogIn Vin(PTB0);
00011 AnalogOut Vout(PTE30);
00012 
00013 Timer t;
00014 int flag;
00015 int q;
00016 int C1=0x0E;    //configurar el lcd para mostrar el guin bajo
00017 int C2=0x0C;    //configurar el lcd para no mostrar el guion bajo
00018 int s=0;       //set point
00019 int p=0;       //ganancia proporcional
00020 int i=0;       //ganancia integral
00021 int d=0;       //ganancia derivativa
00022 int k=0;       //variable de control}
00023 int err, med, yr, ap, ai, ad, err_v, cycle;
00024 int pid;
00025 int viejo=0;
00026 int cont=0;
00027 
00028 void def_posicion(int h){
00029     if (h==0){
00030     lcd.locate(3,0); lcd.printf("%d",s);
00031     lcd.locate(3,0); 
00032     }
00033     else if(h==1){
00034     lcd.locate(11,0);   lcd.printf("%d",p);
00035     lcd.locate(11,0);   
00036     }
00037     else if (h==2){
00038     lcd.locate(3,1); lcd.printf("%d",i);
00039     lcd.locate(3,1); 
00040     }
00041     else {
00042     lcd.locate(11,1);   lcd.printf("%d",d);
00043     lcd.locate(11,1);  
00044     }
00045     }
00046 
00047 
00048 void incremento (int r) {            //Funcion que incremente los valores 
00049 if(k==0){
00050         s+=r;
00051         if(s<=0){
00052         s=0;
00053         }
00054         else if (s>=999){
00055         s=999;
00056         }
00057         lcd.locate(3,0);            
00058         lcd.printf("%d ",s);
00059       }
00060 else if(k==1){
00061         p+=r;
00062         if(p<=0){
00063         p=0;
00064         }
00065         else if (p>=999){
00066         p=999;
00067         }
00068         lcd.locate(11,0);            
00069         lcd.printf("%d ",p);
00070       }
00071 else if(k==2){
00072         i+=r;
00073         if(i<=0){
00074         i=0;
00075         }
00076         else if (i>=999){
00077         i=999;
00078         }
00079         lcd.locate(3,1);            
00080         lcd.printf("%d ",i);  
00081       }
00082 else{
00083         d+=r;
00084         if(d<=0){
00085         d=0;
00086         }
00087         else if (d>=999){
00088         d=999;
00089         }
00090         lcd.locate(11,1);            
00091         lcd.printf("%d ",d);
00092     }
00093  }
00094  
00095  
00096 void star_patch1(void){  //funcion que imprime los caracteres que no van a variar en el display
00097 lcd.cls();
00098 lcd.locate(8,0);
00099 lcd.printf("Kp=%d",p);
00100 lcd.locate(0,1);
00101 lcd.printf("Ki=%d",i);
00102 lcd.locate(8,1);
00103 lcd.printf("Kd=%d",d);
00104 lcd.writeCommand(C1);   //permite que el cursor se vea y sea intermitente
00105 lcd.locate(0,0); 
00106 lcd.printf("Sp=%d",s);
00107 }
00108 
00109 void star_patch2(void){  //funcion que imprime los caracteres que no van a variar en el display
00110 lcd.writeCommand(C2);
00111 lcd.cls();
00112 lcd.printf("***INICIA PID***");    lcd.printf("***************");
00113 wait(2);
00114 lcd.cls();
00115 lcd.printf("Er%=d",err);
00116 lcd.locate(8,0);    lcd.printf("Me=%d",med); 
00117 lcd.locate(0,1);    lcd.printf("Sp=%d",s);
00118 lcd.locate(8,1);    lcd.printf("Co=%d",pid);
00119 wait(3);
00120 }
00121 
00122 int main(){
00123 star_patch1();
00124 viejo=0;
00125 cont=0;
00126 while(1){
00127     if(k>=3 && salto.falling()){
00128     k=0;    
00129     def_posicion(k);
00130     }  
00131     if(salto.falling()){  
00132     ++k;
00133     def_posicion(k);
00134     }
00135     else if(set.falling()){
00136         break;
00137     }
00138     viejo=conteo.getPulses()-cont;
00139     cont=conteo.getPulses();
00140     if(viejo==0){
00141     //no hace nada
00142     }
00143     else {
00144     incremento(viejo);
00145     }}
00146     
00147 star_patch2();
00148 
00149 while (1){
00150 
00151 med=Vin.read()*1000;
00152 err = (s-med);
00153 ap = k*err;           
00154 ai =(i*err)+ai;    //calculo de la integral del error
00155 // se verifica que la accion integral no sea muy grande
00156 ad = d*(err-err_v); //calculo de la accion derivativa
00157 pid = (ap+ai+ad);
00158 //****se muestran las variables******************************************
00159 if (pid > 999){
00160 pid=1000;
00161 } 
00162 else if (pid<0){
00163 pid=0;
00164 }
00165 Vout.write(pid/1000);
00166 if(flag==0){
00167 t.start();
00168 flag=1;
00169 }
00170 if (t>0.3){
00171 lcd.locate(3,0);
00172 lcd.printf("     ");
00173 lcd.locate(3,0);
00174 lcd.printf("%d",err);
00175 lcd.locate(11,0);
00176 lcd.printf("     ");
00177 lcd.locate(11,0);
00178 lcd.printf("%d",med);
00179 lcd.locate(3,1);
00180 lcd.printf("     ");
00181 lcd.locate(3,1);
00182 lcd.printf("%d",s);
00183 lcd.locate(11,1);
00184 lcd.printf("     ");
00185 lcd.locate(11,1);
00186 lcd.printf("%d ",pid);
00187 t.reset();
00188 flag=0;
00189 wait(.1);
00190 }
00191 // se actualizan las variables *******************************************
00192 err_v = err;
00193 }
00194 }