main.cpp

00001 #include "mbed.h"
00002 #include <stdio.h>
00003 #include "QEI.h"
00004 #include "TextLCD.h"
00005 #include "string.h"
00006 
00007 
00008 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00009 QEI encoder (PTA13, PTD5, NC, 624);
00010 AnalogIn y(PTB3);//entrada analoga
00011 AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer
00012 
00013 Serial usart(PTE0,PTE1);  //puertos del FRDM para el modem
00014 Serial pc(USBTX,USBRX); //puertos del PC
00015 
00016 DigitalOut led1(LED1);
00017 DigitalOut led2(LED2);
00018 DigitalOut led3(LED3);
00019 
00020 DigitalIn button3(PTC16);//cambia ingreso de  los 4 parametros
00021 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop
00022 
00023 //codigos movimiento del curzor
00024 
00025 //int C1=0x0E; // solo muestra el curzor
00026 int C2=0x18; // desplaza izquierda
00027 int C3=0x1A; // desplaza derecha
00028 int C4=0x0C; // quito cursor bajo
00029 
00030 int C1=0x0F;
00031 float pid,o,ai,ad,ap,med,err;
00032 float err_v,med_v;
00033 int sp=0,ki=0,kp=0,kd=0;
00034 int j,k;
00035 char buffer[4];// TAMAÑO DEL BUFER
00036 Timer t;   //VALOR DEL TIEMPO
00037 int count;
00038 int contador;
00039 int i = 0;
00040 int c=0;
00041 char cc;
00042 char caux[]="0000";
00043 int readBuffer(char *buffer,int count)   //esta funcion lee un bufer de datos
00044 {
00045     int ij=0; 
00046     t.start();    //CUENTA EL TIEMPO DE CONEXION E INICIA
00047     while(1) {
00048         while (usart.readable()) {
00049             //char
00050             cc = usart.getc();
00051             if (cc == '\r' || cc == '\n') cc = '$';//si se envia fin de linea o de caracxter inserta $
00052             buffer[ij++] = cc;//mete al bufer el caracter leido
00053             if(ij > count)break;//sale del loop si ya detecto terminacion
00054         }
00055         if(ij > count)break;
00056         if(t.read() > 1) {  //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
00057             t.stop();
00058             t.reset();
00059             break;
00060         }
00061     }
00062      return 0;
00063 }
00064  
00065 void cleanBuffer(char *buffer, int count)  //esta funcion limpia el bufer
00066 {
00067     for(int i=0; i < count; i++) {
00068         buffer[i] = '\0';
00069     }
00070 }
00071 void encoderdatos()
00072 {
00073     
00074     int pos=1;
00075     int cambio=0, diferencia=0;
00076 
00077     while(1)
00078         {
00079                 
00080         diferencia=encoder.getPulses()-cambio;
00081         cambio=encoder.getPulses();
00082         if (diferencia==0)
00083             {
00084                 //nada
00085             }
00086         else if(diferencia>0)
00087             {
00088                 if(pos==1)
00089                 {
00090                     
00091                     if(sp+diferencia>=999)
00092                     {
00093                         sp=999;
00094                         lcd.locate(3,0);
00095                         lcd.printf("    ");
00096                         lcd.locate(3,0);
00097                         lcd.printf("%d", sp);                        
00098                     }
00099                     else
00100                     {
00101                         sp+=diferencia;
00102                         lcd.locate(3,0);
00103                         lcd.printf("%d", sp); 
00104                         
00105                         
00106                     }
00107                 }
00108                 else if(pos==2)
00109                 {
00110                     if(kp+diferencia>=999)
00111                     {
00112                         kp=999;
00113                         lcd.locate(11,0);
00114                         lcd.printf("    ");
00115                         lcd.locate(11,0);
00116                         lcd.printf("%d", kp);
00117                     }
00118                     else
00119                     {
00120                         kp+=diferencia;
00121                         lcd.locate(11,0);
00122                         lcd.printf("%d", kp);
00123                     }
00124                 }
00125                 else if(pos==3)
00126                 {
00127                     if(ki+diferencia>=999)
00128                     {
00129                         ki=999;
00130                         lcd.locate(3,1);
00131                         lcd.printf("    ");
00132                         lcd.locate(3,1);
00133                         lcd.printf("%d", ki);
00134                     }
00135                     else
00136                     {
00137                         ki+=diferencia;
00138                         lcd.locate(3,1);
00139                         lcd.printf("%d", ki);
00140                     }
00141                 }
00142                 else if(pos==4)
00143                 {
00144                     if(kd+diferencia>=999)
00145                     {
00146                         kd=999;
00147                         lcd.locate(11,1);
00148                         lcd.printf("    ");
00149                         lcd.locate(11,1);
00150                         lcd.printf("%d", kd);
00151                     }
00152                     else
00153                     {
00154                         kd+=diferencia;
00155                         lcd.locate(11,1);
00156                         lcd.printf("%d", kd);
00157                     }
00158                 }
00159             }
00160         else if(diferencia<0)
00161             {
00162                 if(pos==1)
00163                 {
00164                     if(sp+diferencia<0)
00165                     {
00166                         //No ocurre nada
00167                     }
00168                     else
00169                     {
00170                         sp+=diferencia;
00171                         lcd.locate(3,0);
00172                         lcd.printf("    ");
00173                         lcd.locate(3,0);
00174                         lcd.printf("%d", sp);
00175                     }
00176                 }
00177                 else if(pos==2)
00178                 {
00179                     if(kp+diferencia<0)
00180                     {
00181                         //No ocurre nada
00182                     }
00183                     else
00184                     {
00185                         kp+=diferencia;
00186                         lcd.locate(11,0);
00187                         lcd.printf("    ");
00188                         lcd.locate(11,0);
00189                         lcd.printf("%d", kp);
00190                     }
00191                 }
00192                 else if(pos==3)
00193                 {
00194                     if(ki+diferencia<0)
00195                     {
00196                         //No ocurre nada
00197                     }
00198                     else
00199                     {
00200                         ki+=diferencia;
00201                         lcd.locate(3,1);
00202                         lcd.printf("    ");
00203                         lcd.locate(3,1);
00204                         lcd.printf("%d", ki);
00205                     }
00206                 }
00207                 else if(pos==4)
00208                 {
00209                     if(kd+diferencia<0)
00210                     {
00211                         //No ocurre nada
00212                     }
00213                     else
00214                     {
00215                         kd+=diferencia;
00216                         lcd.locate(11,1);
00217                         lcd.printf("    ");
00218                         lcd.locate(11,1);
00219                         lcd.printf("%d", kd);
00220                     }
00221                 }
00222             }
00223         if (!button3)  //cambia la posicion de ingreso de parametros
00224             {
00225                 led3 =!led3;
00226                 if(pos==4)
00227                 {
00228                     pos=1;
00229                     lcd.locate(3,0);
00230                     lcd.printf("%d", sp);
00231                 }
00232                 else if (pos==1)
00233                 {
00234                     pos++;
00235                     lcd.locate(11,0);
00236                     lcd.printf("%d", kp);
00237                 }
00238                 else if(pos==2)
00239                 {
00240                     pos++;
00241                     lcd.locate(3,1);
00242                     lcd.printf("%d", ki);
00243                 }
00244                 else if(pos==3)
00245                 {
00246                     pos++;
00247                     lcd.locate(11,1);
00248                     lcd.printf("%d", kd);
00249                 }
00250                 wait(0.25);
00251     
00252             }
00253         
00254         
00255         if (!button4)
00256             {
00257                 break;     //sale del bucle si pisan suiche4
00258             }
00259         
00260         wait(0.01); 
00261     }
00262     //return spnum,kinum,kpnum,kdnum;
00263     
00264 }    
00265 
00266 int main()
00267     {
00268     
00269 
00270     lcd.locate(0,1);
00271     lcd.printf("**Control PID**");
00272     wait(2);
00273     lcd.cls(); // Borrar Pantalla
00274     lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
00275 
00276     lcd.locate(8,0);
00277     lcd.printf("Kp=%d",kp);
00278     lcd.locate(0,1);
00279     lcd.printf("Ki=%d",ki);
00280     lcd.locate(8,1);
00281     lcd.printf("Kd=%d",kd);
00282     lcd.locate(0,0);
00283     lcd.printf("Sp=%d",sp);
00284     wait(0.2);
00285     encoderdatos();
00286     wait(0.1);
00287     
00288     usart.baud(9600);   // asigno baudios y configuro puerto serie de la usart
00289     usart.format(8,Serial::None,1);
00290     //
00291       
00292      
00293     //
00294     //sp=valores[0];
00295 
00296     wait(0.001);
00297     if(sp<256)
00298     {
00299         usart.putc(sp);
00300         usart.putc(0);
00301         
00302     }
00303     if(sp>255)
00304     {
00305         j=sp/256;
00306         k=sp-j*256;
00307         usart.putc(k);
00308         usart.putc(j);
00309         
00310     }
00311     wait(0.001);
00312     if(kp<256)
00313     {
00314         usart.putc(kp);
00315         usart.putc(0);
00316         
00317     }
00318     if(kp>255)
00319     {
00320         j=kp/256;
00321         k=kp-j*256;
00322         usart.putc(k);
00323         usart.putc(j);
00324         
00325     }
00326     wait(0.001);
00327     if(ki<256)
00328     {
00329         
00330         usart.putc(ki);
00331         usart.putc(0);
00332         
00333     }
00334     if(ki>255)
00335     {
00336         j=ki/256;
00337         k=ki-j*256;
00338         usart.putc(k);
00339         usart.putc(j);
00340         
00341     }
00342     wait(0.001);
00343     if(kd<256)
00344     {
00345         usart.putc(kd);
00346         usart.putc(0);
00347         
00348     }
00349     if(sp>255)
00350     {
00351         j=kd/256;
00352         k=kd-j*256;
00353         usart.putc(k);
00354         usart.putc(j);
00355         
00356     }
00357     
00358     //Transicion
00359     lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
00360     lcd.cls(); //borra la pantalla
00361     lcd.printf("   GUARDADOS!");
00362     wait(1);
00363     lcd.cls();
00364     lcd.printf(" INICIA EL PID");
00365     wait(1);
00366     // se imprimen los parches del control  *****************************************
00367     lcd.cls();
00368     lcd.printf("Er=%3.0f",err);
00369     lcd.locate(8,0);
00370     lcd.printf("Me=%3.0f",med);
00371     lcd.locate(0,1);
00372     lcd.printf("Sp=%3.0f",sp);
00373     lcd.locate(8,1);
00374     lcd.printf("Co=%3.0f",pid);
00375     wait(1);
00376     
00377     //ciclo control
00378     
00379     while(1)
00380     {
00381         //
00382         if (usart.readable())
00383         {   
00384             cleanBuffer(buffer,4);
00385             readBuffer(buffer,4);
00386             pc.printf("buffer= %s\n\r ",buffer);  //imprime el bufer
00387         
00388             if ('h' == buffer[0])
00389             {
00390                 lcd.cls();
00391                 lcd.locate(3,1);
00392                 lcd.printf("hola: %s",buffer);  
00393                 wait(1);
00394                 lcd.cls();
00395                 //cleanBuffer(buffer,4);
00396                 //sp=88;
00397                 for(int ii=1; ii < 4; ii++){
00398                     caux[ii-1]=buffer[ii];
00399                     
00400                 }
00401                 //sp=caux[2];
00402                 
00403             }
00404         //sp=strtol(caux,null,2);
00405              
00406         }
00407         
00408 
00409         //
00410         med = y.read()*999;
00411         err = (sp-med);  //se calcula el error
00412         ap = kp*err*0.01f;     //se calcula la accion proporcinal
00413         ai =(ki*err*0.01f)+ai;    //calculo de la integral del error
00414         ad = kd*(err-err_v)*0.01f; //calculo de la accion derivativa
00415         pid = (ap+ai+ad);
00416         // se verifica que pid sea positivo **************************************
00417         if(pid<=0)
00418         {
00419             pid=0;
00420         }
00421 
00422         // se verifica que pid sea menor o igual la valor maximo *****************
00423         if (pid > 999)
00424         {
00425             pid=999;
00426         }
00427 
00428        
00429         //se muestran las variables******************************************
00430         lcd.locate(3,0);
00431         lcd.printf("    ");
00432         lcd.locate(3,0);
00433         lcd.printf("%3.0f",err);
00434         lcd.locate(11,0);
00435         lcd.printf("   ");
00436         lcd.locate(11,0);
00437         lcd.printf("%3.0f",med);
00438         lcd.locate(3,1);
00439         lcd.printf("   ");
00440         lcd.locate(3,1);
00441         lcd.printf("%d",sp);
00442         lcd.locate(11,1);
00443         lcd.printf("   ");
00444         lcd.locate(11,1);
00445         lcd.printf("%3.0f",pid);
00446            
00447             
00448         
00449 
00450         //Normalizacion de la salida
00451         // se actualizan las variables *******************************************
00452         err_v = err;
00453         if(err_v<0)
00454         {
00455             err_v=err_v*(-1);   
00456         }
00457 
00458         if(err_v<256)
00459         {
00460             usart.putc(err_v);
00461             usart.putc(0);
00462         
00463         }
00464         
00465         if(err_v>255)
00466         {
00467             j=err_v/256;
00468             k=err_v-j*256;
00469             usart.putc(k);
00470             usart.putc(j);
00471         
00472         }
00473         //wait(0.1);
00474         
00475         med_v = med;
00476         //wait(0.1);
00477         if(med_v<256)
00478         {
00479             usart.putc(med_v);
00480             usart.putc(0);
00481         
00482         }
00483         
00484         if(med_v>255)
00485         {
00486             j=med_v/256;
00487             k=med_v-j*256;
00488             usart.putc(k);
00489             usart.putc(j);
00490         
00491         }
00492         //GSM.putc(err_v);
00493         o = pid/999;
00494         u.write(o);
00495         //  se envia el valor pid a puerto analogico de salida (D/A) **************
00496         
00497         //  se repite el ciclo
00498         wait_ms(300);
00499     }
00500         
00501     
00502 }