main.cpp

00001 // Programa para ejecutar un control PID que lee las constantes del controlador desde la UART,
00002 // que son enviadas por Bluetooth desde un proyecto de App Inventor ejecutado en un dispositivo
00003 // Android como cadena de caracteres. Cuando se inicia el proceso de control se envían los 
00004 // parámetros del mismo por Bluetooth hacia el dispositivo Android, el proyecto en App Inventor
00005 // permite recibir la cadena de parámetros y separarlos para asignarlos a su respectivo indicador.
00006 
00007 // Oswaldo Andrés Giraldo Giraldo - C.C.: 1152458465
00008 // Héctor Andrés Hoyos Ceballos - C.C.: 1039466317
00009 // Jose Fernando Montoya Vargas - C.C.: 1039468676
00010 // María Fernanda Villa Tamayo - C.C.: 1152457490
00011 
00012 #include "mbed.h"
00013 #include "TextLCD.h"
00014 
00015 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // RS, E, D4 - D7
00016 Serial andr(USBTX, USBRX); // TX, RX
00017 Serial device(PTE0, PTE1);  // TX, RX
00018 
00019 AnalogIn y(PTB3); // Entrada análoga
00020 AnalogOut u(PTE30); // Salida análoga
00021 DigitalOut led1(LED1);
00022 DigitalOut led2(LED2);
00023 DigitalOut led3(LED3);
00024 
00025 // Códigos movimiento del cursor.
00026 //int C1=0x0E; // Sólo muestra el cursor.
00027 int C2=0x18; // Desplaza hacia la izquierda.
00028 int C3=0x1A; // Desplaza hacia la derecha.
00029 int C4=0x0C; // Quita cursor bajo.
00030 int C1=0x0F;
00031 
00032 float pid,o,ai,ad,ap,med,err,medr;
00033 float err_v;
00034 int spnum = 0, kinum = 0,kpnum = 0, kdnum = 0;
00035 char outmed[16], outmedn[16], outpid[16], outpidn[16], outspnum[16], outspnumn[16], outerr[16], outerrn[16], varout[40];
00036 
00037 Timer t;
00038 char buffer[4], resp[4], ind;
00039 int count, i;
00040 int k = 0, j = 0, num = 0;
00041 
00042 // Reverses a string 'str' of length 'len'
00043 // driver program to test above funtion.
00044 void reverse(char *str, int len)
00045 {
00046     int i=0, j=len-1, temp;
00047     while (i<j)
00048     {
00049         temp = str[i];
00050         str[i] = str[j];
00051         str[j] = temp;
00052         i++; j--;
00053     }
00054 }
00055  
00056  // Converts a given integer x to string str[].  d is the number
00057  // of digits required in output. If d is more than the number
00058  // of digits in x, then 0s are added at the beginning.
00059 int intToStr(int x, char str[], int d)
00060 {
00061     int i = 0;
00062     while (x)
00063     {
00064         str[i++] = (x%10) + '0';
00065         x = x/10;
00066     }
00067  
00068     // If number of digits required is more, then
00069     // add 0s at the beginning
00070     while (i < d)
00071         str[i++] = '0';
00072  
00073     reverse(str, i);
00074     str[i] = '\0';
00075     return i;
00076 }
00077 
00078 // Converts a floating point number to string.
00079 void ftoa(float n, char *res, int afterpoint)
00080 {
00081     // Extract integer part
00082     int ipart = (int)n;
00083  
00084     // Extract floating part
00085     float fpart = n - (float)ipart;
00086  
00087     // convert integer part to string
00088     int i = intToStr(ipart, res, 0);
00089  
00090     // check for display option after point
00091     if (afterpoint != 0)
00092     {
00093         res[i] = '.';  // add dot
00094  
00095         // Get the value of fraction part upto given no.
00096         // of points after dot. The third parameter is needed
00097         // to handle cases like 233.007
00098         float fp=10;
00099         fpart =fpart * pow(fp,afterpoint);
00100         
00101         intToStr((int)fpart, res + i + 1, afterpoint);
00102     }
00103 }
00104 
00105 // Lee la cadena de caracteres que llegan a la UART.
00106 int readBuffer(char *buffer,int count)
00107 {
00108     int i=0; 
00109     t.start();  // start timer
00110     while(1) {
00111         while (device.readable()) {
00112             char c = device.getc();
00113             if (c == '\r' || c == '\n') c = '$';
00114             buffer[i++] = c;
00115             if(i > count)break;
00116         }
00117         if(i > count)break;
00118         if(t.read() > 1) {
00119             t.stop();
00120             t.reset();
00121             break;
00122         }
00123     }
00124     return 0;
00125 }
00126 
00127 // Limpia una cadena de caracteres.
00128 void cleanBuffer(char *buffer, int count)
00129 {
00130     for(int i=0; i < count; i++) {
00131         buffer[i] = '\0';
00132     }
00133 }
00134 
00135 int main(void) {
00136     device.baud(9600);// Configura los baudios de la FRDMKL25Z en 9600.
00137     device.format(8,Serial::None,1); // Configura el formato de los datos de la UART.   
00138     lcd.locate(1,1); // Se posiciona en col 1 fil 1.
00139     lcd.printf("**Control PID**");
00140     wait(2);
00141 
00142     lop0: // CICLO DE ASIGNACIÓN DE VARIABLES.
00143         lcd.cls(); // Borrar Pantalla.
00144         lcd.writeCommand(C1);// Escribimos un comando segun el manual del modulo LCD.
00145         lcd.locate(8,0);
00146         lcd.printf("Kp=%d",kpnum); // Constante proporcional.
00147         lcd.locate(0,1);
00148         lcd.printf("Ki=%d",kinum); // Constante integral.
00149         lcd.locate(8,1);
00150         lcd.printf("Kd=%d",kdnum); // Constante derivativa.
00151         lcd.locate(0,0);
00152         lcd.printf("Sp=%d",spnum); // Set point (punto de ajuste)
00153     
00154         while(1){                    
00155             if(device.readable()){ // Si llega algún paquete por la UART.
00156                 readBuffer(buffer,4); // Leer la cadena de caracteres.
00157                 led1 =!led1;
00158                 //andr.putc(device.getc());
00159                 andr.printf("%s\r\n",buffer);
00160                 ind = buffer[0]; // Las cadenas que llegan tienen un indicador en la primera posición.
00161             
00162                 if (ind == '4') { // Si llega una cadena con el indicador '4' (Sp).
00163                     resp[0] = buffer[1]; // Toma el valor de la constante ingresada.
00164                     resp[1] = buffer[2];
00165                     resp[2] = buffer[3];  
00166                     spnum = strtod(resp,NULL); // Asigna la cadena a un entero.
00167                     //andr.printf("%s\r\n",resp);
00168                     lcd.locate(3,0);
00169                     lcd.printf("    ");
00170                     lcd.locate(3,0);
00171                     lcd.printf("%d", spnum); // Imprime la referencia.
00172                     led1 =!led1;
00173                 }
00174                 
00175                 if (ind == '1') { // Si llega una cadena con el indicador '1' (Kp).
00176                     resp[0] = buffer[1]; // Toma el valor de la constante ingresada.
00177                     resp[1] = buffer[2];
00178                     resp[2] = buffer[3];  
00179                     kpnum = strtod(resp,NULL); // Asigna la cadena a un entero.
00180                     //pc.printf("%s\r\n",resp);
00181                     lcd.locate(11,0);
00182                     lcd.printf("    ");
00183                     lcd.locate(11,0);
00184                     lcd.printf("%d", kpnum); // Imprime Kp.
00185                     led1 =!led1;
00186                 }
00187                 
00188                 if (ind == '2') { // Si llega una cadena con el indicador '2' (Ki).
00189                     resp[0] = buffer[1]; // Toma el valor de la constante ingresada.
00190                     resp[1] = buffer[2];
00191                     resp[2] = buffer[3];  
00192                     kinum = strtod(resp,NULL); // Asigna la cadena a un entero.
00193                     //pc.printf("%s\r\n",resp);                
00194                     lcd.locate(3,1);
00195                     lcd.printf("    ");
00196                     lcd.locate(3,1);
00197                     lcd.printf("%d", kinum); //Imprime Ki.
00198                     led1 =!led1;
00199                 }
00200                 
00201                 if (ind == '3') { // Si llega una cadena con el indicador '3' (Kd).
00202                     resp[0] = buffer[1]; // Toma el valor de la constante ingresada.
00203                     resp[1] = buffer[2];
00204                     resp[2] = buffer[3];  
00205                     kdnum = strtod(resp,NULL); // Asigna la cadena a un entero.
00206                     //pc.printf("%s\r\n",resp);
00207                     lcd.locate(11,1);
00208                     lcd.printf("    ");
00209                     lcd.locate(11,1);
00210                     lcd.printf("%d", kdnum); // Imprime Kd.
00211                     led1 =!led1;
00212                 }
00213                 
00214                 if (ind == '5') { // Si llega una cadena con el indicador '5' (Borrar).
00215                     led1 =!led1;
00216                     led2 =!led2;
00217                     led3 =!led3;
00218                     kpnum = 0; // Borra las constantes del controlador.
00219                     kinum = 0;
00220                     kdnum = 0;
00221                     spnum = 0;
00222                     lcd.cls(); // Borrar Pantalla.
00223                     lcd.locate(1,1); // Se posiciona en col 1 fil 1.
00224                     lcd.printf("DATOS BORRADOS");
00225                     wait(2);
00226                     lcd.cls(); // Borrar Pantalla.
00227                     lcd.writeCommand(C1);// Escribimos un comando segun el manual del modulo LCD.
00228                     lcd.locate(8,0);
00229                     lcd.printf("Kp=%d",kpnum);
00230                     lcd.locate(0,1);
00231                     lcd.printf("Ki=%d",kinum);
00232                     lcd.locate(8,1);
00233                     lcd.printf("Kd=%d",kdnum);
00234                     lcd.locate(0,0);
00235                     lcd.printf("Sp=%d",spnum);
00236                     led2 =!led2;
00237                     led3 =!led3;
00238                 }
00239                 
00240                 if (ind == '7') { // Si llega una cadena con el indicador '7' (Detener).
00241                     led1 =!led1; // No hacer nada, pues el controlador no se ha ejecutado.
00242                 }
00243                 
00244                 if (ind == '6') { // Si llega una cadena con el indicador '6' (Iniciar).
00245                     led1 =!led1;
00246                     led3 =!led3;
00247                     
00248                     // Transición.
00249                     lcd.writeCommand(C4);//Escribimos un comando segun el manual del modulo LCD para quitar cursor bajo.
00250                     lcd.cls(); // Borra la pantalla.
00251                     lcd.locate(1,1); // Se posiciona en col 1 fil 1.
00252                     lcd.printf("DATOS GUARDADOS");
00253                     wait(1);
00254                     lcd.cls();
00255                     lcd.locate(1,1); // Se posiciona en col 1 fil 1.
00256                     lcd.printf("INICIO DEL PID");
00257                     wait(2);
00258                     
00259                     // Se imprimen los parches del control.
00260                     lcd.cls();
00261                     lcd.printf("Er=%3.0f",err);
00262                     lcd.locate(8,0);
00263                     lcd.printf("Me=%3.0f",med);
00264                     lcd.locate(0,1);
00265                     lcd.printf("Sp=%3.0f",spnum);
00266                     lcd.locate(8,1);
00267                     lcd.printf("Co=%3.0f",pid);
00268                     wait(1);
00269                     
00270                     // CICLO PRINCIPAL CONTROLADOR PID.
00271                     lop1:
00272                         med = y.read()*999; // Se toma la medida actual por el puerto analógico.
00273                         //andr.printf("%f", med);
00274                         err = (spnum-med);  // Se calcula el error.
00275                         //andr.printf("%f", err);
00276                         ap = kpnum*err*0.01f; // Se calcula la acción proporcinal.
00277                         ai =(kinum*err*0.01f)+ai; // Cálculo de la integral del error.
00278                         ad = kdnum*(err-err_v)*0.01f; // Cálculo de la acción derivativa.
00279                         pid = (ap+ai+ad); // Se actualiza el valor del la acción de control.     
00280                         //andr.printf("%f", pid);
00281                                             
00282                         // Se verifica que pid sea positivo.
00283                         if(pid<=0) {
00284                             pid=0;
00285                         }
00286                     
00287                         // Se verifica que pid sea menor o igual la valor máximo.
00288                         if (pid > 999) {
00289                             pid=999;
00290                         }
00291                         
00292                         // Se envían los datos del control como caracteres.
00293                         cleanBuffer(varout, 40);
00294                         cleanBuffer(outerr, 16);
00295                         cleanBuffer(outmed, 16);
00296                         cleanBuffer(outpid, 16);
00297                         cleanBuffer(outspnum, 16);
00298                         
00299                         ftoa(spnum, outspnum, 4); // Se convierte el Set point a caracter.
00300                         
00301                         if (err < 0 && err > -1){ // Se convierte el Error a caracter.
00302                             err = (-1)*err;
00303                             strcat(outerr, "0");
00304                             ftoa(err, outerrn, 4);
00305                             strcat(outerr, outerrn);
00306                         }
00307                         else if (err <= -1){
00308                             err = (-1)*err;
00309                             ftoa(err, outerr, 4);
00310                         }
00311                         else if (err > 0 && err < 1){
00312                             strcat(outerr, "0");
00313                             ftoa(err, outerrn, 4);
00314                             strcat(outerr, outerrn);
00315                         }
00316                         else{
00317                             ftoa(err, outerr, 4);
00318                         }
00319                         
00320                         if (med < 1){ // Se convierte la Medida a caracter.
00321                             strcat(outmed, "0");
00322                             ftoa(med, outmedn, 4);
00323                             strcat(outmed, outmedn);
00324                         }
00325                         else{
00326                             ftoa(med, outmed, 4);
00327                         }
00328                         
00329                         if (pid < 1){ // Se convierte la Acción de control a caracter.
00330                             strcat(outpid, "0");
00331                             ftoa(pid, outpidn, 4);
00332                             strcat(outpid, outpidn);
00333                         }
00334                         else{
00335                             ftoa(pid, outpid, 4);
00336                         }
00337                             
00338                         strcat(varout, " /"); // Se prepara la cadena a ser enviada, se separa cada constante del control por "/".
00339                         strcat(varout, outspnum);
00340                         strcat(varout, "/");
00341                         strcat(varout, outerr);
00342                         strcat(varout, "/");
00343                         strcat(varout, outmed);
00344                         strcat(varout, "/");
00345                         strcat(varout, outpid);
00346                         
00347                         device.printf(varout);     
00348                         andr.printf("%s\n", varout);
00349                     
00350                         // Se muestran las variables en la LCD.
00351                         lcd.locate(3,0);
00352                         lcd.printf("    ");
00353                         lcd.locate(3,0);
00354                         lcd.printf("%3.0f",err);
00355                         lcd.locate(11,0);
00356                         lcd.printf("   ");
00357                         lcd.locate(11,0);
00358                         lcd.printf("%3.0f",med);
00359                         lcd.locate(3,1);
00360                         lcd.printf("   ");
00361                         lcd.locate(3,1);
00362                         lcd.printf("%3.0d",spnum);
00363                         lcd.locate(11,1);
00364                         lcd.printf("   ");
00365                         lcd.locate(11,1);
00366                         lcd.printf("%3.0f",pid);
00367                     
00368                         // Normalización de la salida.
00369                         // Se actualizan las variables.
00370                         err_v = err;
00371                         o = pid/999;
00372                         u.write(o); // Se envía el valor pid a puerto analogico de salida (D/A).
00373                         
00374                         // Se repite el ciclo.
00375                         wait_ms(300);
00376                         
00377                         if(device.readable()){ // Si llega alguna cadena de caracteresp por la UART, principalmente para detener el controlador.
00378                             //andr.putc(device.getc());
00379                             readBuffer(buffer,4);
00380                             //andr.printf("%s\r\n",buffer);
00381                             ind = buffer[0];
00382                             
00383                             if (ind == '7'){ // Si llega una cadena con el indicador '7' (Dentener).
00384                                 andr.printf("%s", ind);
00385                                 pid = 0; // Regresa las variables del proceso de control a cero.
00386                                 ai = 0;
00387                                 ad = 0;
00388                                 ap = 0;
00389                                 med = 0;
00390                                 err = 0;
00391                                 err_v = 0;
00392                                 
00393                                 lcd.cls(); // Borrar Pantalla
00394                                 lcd.locate(1,1); //se posiciona en col 1 fil 1
00395                                 lcd.printf("PID DETENIDO");
00396                                 wait(2);
00397                                 lcd.cls(); // Borrar Pantalla
00398                                 lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
00399                                 led3 =!led3;
00400                                 
00401                                 lcd.locate(8,0);
00402                                 lcd.printf("Kp=%d",kpnum);
00403                                 lcd.locate(0,1);
00404                                 lcd.printf("Ki=%d",kinum);
00405                                 lcd.locate(8,1);
00406                                 lcd.printf("Kd=%d",kdnum);
00407                                 lcd.locate(0,0);
00408                                 lcd.printf("Sp=%d",spnum);
00409                             }
00410                             cleanBuffer(buffer,4);
00411                             goto lop0; // Si en efecto se detiene el PID vuelve al ciclo de asignación de variables.
00412                         }
00413                         goto lop1; // Si no se detiene el PID entonces vuelve al ciclo de control.
00414                 }
00415                 cleanBuffer(buffer,4);
00416             }
00417             //andr.putc(device.getc());  
00418         }
00419 }