main.cpp

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