main.cpp

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