main.cpp

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