main.cpp

00001 #include "mbed.h"
00002 #include "TextLCD.h"
00003 #include "QEI.h"
00004 
00005   
00006 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00007 QEI leftQei(PTA1, PTA2, NC, 624);
00008  
00009 AnalogIn Vin(PTC2);
00010 
00011 AnalogIn y(PTB0); //entrada análogica de la planta RC
00012 AnalogOut u(PTE30);
00013 
00014 DigitalOut l1(LED1);
00015 DigitalOut l2(LED2);
00016 DigitalOut l3(LED3);
00017 DigitalIn b3(PTD4);
00018 DigitalIn b4(PTC17);
00019 
00020 int spnum=0,kpnum=0,kinum=0,kdnum=0,k=1;
00021 int bandera;
00022 Timer t;
00023 float tiempo;
00024 
00025 //codigos movimiento del curzor
00026 
00027 //int C1=0x0E; // solo muestra el curzor
00028 int C2=0x18; // desplaza izquierda
00029 int C3=0x1A; // desplaza derecha
00030 int C4=0x0C; // quito cursor bajo
00031 
00032 int C1=0x0F;
00033 int err, med, yr, pid, ap, ai, ad, err_v, cycle,i;
00034 float pidn;
00035 int pos=1;
00036 //int spnum=500, kinum=2, kpnum=1, kdnum=0, pos=1;
00037 //int incremento=1,flagt=0;
00038 
00039 int main()
00040 {
00041   lcd.cls(); // Borrar Pantalla
00042     lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
00043    //A continuación localizamos en el LCD los nombres de las variables del PID.
00044     lcd.locate(8,0); 
00045     lcd.printf("Kp=%d",kpnum);
00046     lcd.locate(0,1);
00047     lcd.printf("Ki=%d",kinum);
00048     lcd.locate(8,1);
00049     lcd.printf("Kd=%d",kdnum);
00050     lcd.locate(0,0);
00051     lcd.printf("Sp=%d",spnum);
00052     
00053        while(1)
00054        {
00055         while(k==1) // k será la variable que nos localice en que posición del LCD nos encontramos.
00056         {
00057             spnum=leftQei.getPulses(); // Comando que permite aumentar o disminuir el valor de "SP" por medio del encoder.
00058             if (spnum>=0 & spnum<=999) //acotamos los valores de "SP" entre 0 y 999.
00059             {
00060                 lcd.locate(3,0);
00061                 lcd.printf("    ");
00062                 lcd.locate(3,0);
00063                 lcd.printf("%i",spnum); //imprime un número para "SP"
00064                 wait(0.2);        
00065             }
00066             if (!b3) //Al oprimir el pulsador del encoder, permita el cambio de posición desde "SP" a "KP" 
00067             {                
00068                 k+=1;
00069                 wait(0.25);
00070                 lcd.locate(11,0);
00071                 lcd.printf("%d",kpnum);               
00072             }
00073 
00074         }
00075 
00076         while(k==2) // nos encontramos en la posición 2 (KP).
00077         {   
00078             wait(0.2);                                
00079             kpnum=leftQei.getPulses()-spnum;
00080             if (kpnum>=0 & kpnum<=999)
00081             {                       
00082                 lcd.locate(11,0);
00083                 lcd.printf("    ");
00084                 lcd.locate(11,0);
00085                 lcd.printf("%i",kpnum);
00086                 wait(0.2);
00087             }  
00088             
00089             if (!b3)
00090             {
00091                 kpnum=leftQei.getPulses();
00092                 k+=1;
00093                 wait(0.25);
00094                 lcd.locate(3,1); 
00095                 lcd.printf("%d",kinum);
00096             }
00097         }
00098 
00099         while(k==3) // nos encontramos en la posición 3 (KI).
00100         {   
00101             wait(0.2);                     
00102             kinum=leftQei.getPulses()-kpnum;            
00103             if (kinum>=0 & kinum<=999)
00104             {
00105                 lcd.locate(3,1);
00106                 lcd.printf("    ");
00107                 lcd.locate(3,1);
00108                 lcd.printf("%i",kinum);
00109                 wait(0.2);                 
00110             }
00111             if (!b3)
00112             {        
00113                 kinum=leftQei.getPulses();        
00114                 k+=1;
00115                 wait(0.25);
00116                 lcd.locate(11,1); 
00117                 lcd.printf("%d",kdnum);
00118             }
00119         }
00120 
00121         while(k==4)// nos encontramos en la posición 4 (KD).
00122         {
00123             kdnum=leftQei.getPulses()-kinum;
00124             if (kdnum>=0 & kdnum<=999)
00125             {
00126                 lcd.locate(11,1);
00127                 lcd.printf("    ");
00128                 lcd.locate(11,1);
00129                 lcd.printf("%i",kdnum);
00130                 wait(0.2);  
00131             }
00132             if (!b3)
00133             {
00134                 k=1;
00135                 wait(0.25);
00136                 lcd.locate(3,0);
00137                 lcd.printf("%d",spnum);
00138             }
00139 
00140         }
00141         
00142  
00143         if (b4)
00144         {
00145            break;     //sale del bucle si pisan suiche4
00146         }
00147         continue;
00148     }
00149 
00150 
00151     //Transicion
00152     
00153     lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
00154     lcd.cls(); //borra la pantalla
00155     lcd.printf("GUARDADOS!");
00156     wait(1);
00157     lcd.cls();
00158     lcd.printf(" INICIA EL PID");
00159     wait(1);
00160     // se imprimen los parches del control  *****************************************
00161     lcd.cls();
00162     lcd.printf("Er=%d",err);
00163     lcd.locate(8,0);
00164     lcd.printf("Me=%d",med);
00165     lcd.locate(0,1);
00166     lcd.printf("Sp=%d",spnum);
00167     lcd.locate(8,1);
00168     lcd.printf("Co=%d",pid);
00169     wait(2);
00170     
00171     // CICLO PRINCIPAL CONTROLADOR PID
00172     bandera=0;
00173     while(1)
00174     {
00175         med=999*y.read();   //leer puerto analogo y asignar a med
00176         err = (spnum-med);  //se calcula el error
00177 
00178         ap = kpnum*err;     //se calcula la accion proporcinal
00179 
00180         // se verifica que la accion integral no sea muy grande
00181         if(ai<100)
00182         {
00183             ai =(kinum*err)+ai;    //calculo de la integral del error
00184         }
00185 
00186         ad = kdnum*(err-err_v); //calculo de la accion derivativa
00187 
00188         pid = (ap+ai+ad);
00189 
00190         // se verifica que pid sea positivo **************************************
00191         if(pid<=0)
00192         {
00193             pid=0;
00194         }
00195 
00196         // se verifica que pid sea menor o igual la valor maximo *****************
00197         if (pid > 999)
00198         {
00199             pid=999;
00200         }
00201 
00202         // se actualizan las variables *******************************************
00203         err_v = err;
00204 
00205         //se muestran las variables******************************************
00206         if(bandera==0)
00207         {
00208             t.start();
00209             bandera=1;
00210         }
00211         if(t>=0.3)
00212         {
00213             lcd.locate(3,0);lcd.printf("    ");
00214             lcd.locate(3,0);lcd.printf("%d",err);
00215             lcd.locate(11,0);lcd.printf("    ");
00216             lcd.locate(11,0);lcd.printf("%d",med);
00217             lcd.locate(3,1);lcd.printf("    ");
00218             lcd.locate(3,1);lcd.printf("%d",spnum);
00219             lcd.locate(11,1);lcd.printf("    ");
00220             lcd.locate(11,1);lcd.printf("%d",pid);
00221             bandera=0;
00222             t.reset();
00223         }
00224 
00225         //Normalizacion de la salida
00226         pidn=pid/999;
00227         //  se envia el valor pid a puerto analogico de salida (D/A) **************
00228         u.write(pidn);
00229         //  se repite el ciclo
00230         wait(0.005);
00231     }
00232 
00233 }