main.cpp

00001 #include "mbed.h" 
00002 #include "QEI.h"      // Librería del ENCODER GIRATORIO("potenciómetro"+PULSADOR)
00003 #include "TextLCD.h"    // LCD 
00004 #include "DebouncedIn.h"
00005 #include "stdio.h"      //printf, etc
00006 #include "string.h"
00007 #include "stdlib.h"     // atoi
00008 #include "stdint.h"
00009 #include <assert.h>
00010 
00011 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00012 QEI encoder (PTA13, PTD5, NC, 624);
00013 AnalogIn y(PTB3);           //entrada analoga
00014 AnalogOut u(PTE30);         //salida analoga (Imax=1,5mA!!!!)
00015 DigitalOut LedVerde(LED2);
00016 DigitalOut LedRojo(LED1);
00017 DigitalOut LedAzul(LED3);
00018 
00019 DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop
00020 
00021 //****************** CONFIGURACIÓN DE PUERTOS DE COMUNICACIÓN ******************
00022 Serial BLUETOOTH(PTE0,PTE1);  //puertos USART del FRDM para el módulo BLUETOOTH
00023 Serial pc(USBTX,USBRX);
00024 
00025 //********************************** VARIABLES *********************************
00026 int cambio=0, diferencia=0;
00027 float pid,o,ai,ad,ap,med,err;
00028 float err_v;
00029 int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1;
00030 int i,ret;
00031 int n=0;
00032 char *ptr;
00033 char bufferllegada[100];
00034 char bufferenvio[100];
00035 Timer t;
00036 
00037 //********************************** FUNCIONES *********************************
00038 //*********************** LECTURA BUFFER - INTERRUPCIONES **********************
00039 int readBuffer(char *buffer,int count){
00040     int i=0; 
00041     t.start();  // start timer
00042     while(1) {
00043         while (BLUETOOTH.readable()) {
00044             char c = BLUETOOTH.getc();
00045             if (c == '\r' || c == '\n') c = '\0';
00046             buffer[i++] = c;
00047             if(i > count)break;
00048             }
00049             if(i > count)break;
00050             if(t.read() > 1) {
00051                 t.stop();
00052                 t.reset();
00053                 break;
00054                 }
00055         }
00056     wait(0.5);
00057     while(BLUETOOTH.readable()) { 
00058         char c = BLUETOOTH.getc();
00059         }
00060     return 0;
00061     }
00062 
00063 //****************************** BORRADO DE BUFFER *****************************
00064 void cleanBuffer(char *buffer, int count){
00065     for(int i=0; i < count; i++) {
00066         buffer[i] = '\0';
00067     }
00068 }
00069 //******************** SPLIT A STRING BY PARSING STRINGS BY ; ******************
00070 
00071 
00072 int dtmsplit(char *str, const char *delim, char ***array, int *length ) {
00073   int i=0;
00074   char *token;
00075   char **res = (char **) malloc(0 * sizeof(char *));
00076 
00077   /* get the first token */
00078    token = strtok(str, delim);
00079    while( token != NULL )
00080    {
00081         res = (char **) realloc(res, (i + 1) * sizeof(char *));
00082         res[i] = token;
00083         i++;
00084       token = strtok(NULL, delim);
00085    }
00086    *array = res;
00087    *length = i;
00088   return 1;
00089 }
00090 
00091 
00092 //******************************************************************************
00093 //******************************* CICLO PRINCIPAL ******************************
00094 //******************************************************************************
00095 int main()
00096 {
00097     //***************************** BAUDIOS Y BITS *****************************
00098     BLUETOOTH.baud(9600);
00099     BLUETOOTH.format(8,Serial::None,1);
00100     
00101     lcd.locate(0,1);
00102     lcd.printf("**Control PID**");
00103     pc.printf("**Control PID**\r\n");
00104     wait(2);
00105     lcd.cls(); // Borrar Pantalla
00106     lcd.locate(0,0);
00107     lcd.printf("Esperando datos...");
00108     pc.printf("Esperando datos...\r\n");
00109     
00110     while(1){
00111         if (BLUETOOTH.readable()) {
00112             readBuffer(bufferllegada,100);
00113             if(NULL != ( ptr = strstr(bufferllegada,"SP"))){
00114                 printf("Buffer original = [%s] \n\n", bufferllegada);
00115                 //******************** PARTIENDO LA CADENA *********************
00116                 int i;
00117                 char **arr = NULL;
00118                 int count =0;
00119                 int c = dtmsplit(bufferllegada, ";", &arr, &count);
00120                 pc.printf("Found %d tokens.\n", count);
00121                 for (i = 0; i < count; i++){
00122                     pc.printf("string #%d: %s\n", i, arr[i]);
00123                     }
00124                 pc.printf("SP-string=%s\n",arr[1]);
00125                 pc.printf("KP-string=%s\n",arr[3]);
00126                 pc.printf("KI-string=%s\n",arr[5]);
00127                 pc.printf("KD-string=%s\n",arr[7]);
00128                 pc.printf("\n");
00129                 spnum=atoi(arr[1]);          //atoi ->  Convert string to integer C++
00130                 kpnum=atoi(arr[3]);
00131                 kinum=atoi(arr[5]);
00132                 kdnum=atoi(arr[7]);
00133                 pc.printf("SP=%i\n",spnum);
00134                 pc.printf("KP=%i\n",kpnum);
00135                 pc.printf("KI=%i\n",kinum);
00136                 pc.printf("KD=%i\n",kdnum);
00137                 pc.printf("\n");
00138                 free(arr);
00139                 }   //end if(NULL != ( ptr1 = strstr(buffer,"SP"))){
00140                 // My code (tested) in http://stackoverflow.com/questions/9210528/split-string-with-delimiters-in-c
00141                 // http://stackoverflow.com/questions/9054553/atoi-from-string-to-integer-using-char-pointer
00142                 // https://www.tutorialspoint.com/cprogramming/c_pointers.htm
00143             // Impresión de cada parámetros en LCD
00144             lcd.cls(); // Borrar Pantalla
00145             // Imprime SP
00146             lcd.locate(0,0);
00147             lcd.printf("Sp=%d",spnum);
00148             lcd.locate(3,0);                // necesariamente (columna, fila)
00149             lcd.printf("    ");
00150             lcd.locate(3,0);
00151             lcd.printf("%d", spnum);
00152             // Imprime KP
00153             lcd.locate(8,0);                
00154             lcd.printf("Kp=%d",kpnum);
00155             lcd.locate(11,0);
00156             lcd.printf("    ");
00157             lcd.locate(11,0);
00158             lcd.printf("%d", kpnum);
00159             // Imprime KI
00160             lcd.locate(0,1);
00161             lcd.printf("Ki=%d",kinum);
00162             lcd.locate(3,1);
00163             lcd.printf("    ");
00164             lcd.locate(3,1);
00165             lcd.printf("%d", kinum);
00166             // Imprime KD
00167             lcd.locate(8,1);
00168             lcd.printf("Kd=%d",kdnum);
00169             lcd.locate(11,1);
00170             lcd.printf("    ");
00171             lcd.locate(11,1);
00172             lcd.printf("%d", kdnum);
00173             }   //Fin if (BLUETOOTH.readable()) {
00174                 
00175             if (!button4){
00176                 wait(0.1); 
00177                 break;     //sale del bucle si pisan suiche4
00178                 }
00179     }   // FIN while(1)            
00180     
00181 
00182 //Transicion
00183     lcd.cls(); //borra la pantalla
00184     lcd.printf("   GUARDADOS!");
00185     pc.printf("   GUARDADOS!\r\n");
00186     wait(1);
00187     lcd.cls();
00188     lcd.printf(" INICIA EL PID");
00189     pc.printf(" INICIA EL PID\r\n");
00190     wait(1);
00191 // se imprimen los parches del control  *****************************************
00192     lcd.cls();
00193     lcd.printf("Er=%3.0f",err);
00194     lcd.locate(8,0);
00195     lcd.printf("Me=%3.0f",med);
00196     lcd.locate(0,1);
00197     lcd.printf("Sp=%3.0f",spnum);
00198     lcd.locate(8,1);
00199     lcd.printf("Co=%3.0f",pid);
00200     wait(1);
00201 
00202 // CICLO PRINCIPAL CONTROLADOR PID
00203 
00204  while(1){
00205         med = y.read()*999;
00206         err = (spnum-med);  //se calcula el error
00207         ap = kpnum*err*0.01f;     //se calcula la accion proporcinal
00208         ai =(kinum*err*0.01f)+ai;    //calculo de la integral del error
00209         ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa
00210         pid = (ap+ai+ad); 
00211         
00212         // se verifica que pid sea positivo **************************************
00213         if(pid<=0){
00214             pid=0;
00215         }
00216         // se verifica que pid sea menor o igual la valor maximo *****************
00217         if (pid > 999){
00218             pid=999;
00219         }
00220         //se muestran las variables******************************************
00221             lcd.locate(3,0);
00222             lcd.printf("    ");
00223             lcd.locate(3,0);
00224             lcd.printf("%3.0f",err);
00225             lcd.locate(11,0);
00226             lcd.printf("   ");
00227             lcd.locate(11,0);
00228             lcd.printf("%3.0f",med);
00229             lcd.locate(3,1);
00230             lcd.printf("   ");
00231             lcd.locate(3,1);
00232             lcd.printf("%d",spnum);
00233             lcd.locate(11,1);
00234             lcd.printf("   ");
00235             lcd.locate(11,1);
00236             lcd.printf("%3.0f",pid);
00237             
00238             /*
00239             pc.printf("Er=%3.0f\r\n",err);
00240             pc.printf("Me=%3.0f\r\n",med);
00241             pc.printf("Sp=%3.0f\r\n",spnum);
00242             pc.printf("Co=%3.0f\r\n",pid);
00243             */     
00244         //Normalizacion de la salida
00245         // se actualizan las variables *******************************************
00246         //  se envia el valor pid a puerto analogico de salida (D/A) **************
00247         err_v = err;
00248         o = pid/999;
00249         u.write(o);
00250         
00251         //  se repite el ciclo
00252         wait_ms(30);
00253         
00254         //****************** CONCATENA Y ENVÍA POR BLUETOOTH *******************
00255         cleanBuffer(bufferllegada,100);
00256         cleanBuffer(bufferenvio,100);
00257         
00258         char str1[]="SEND";
00259         char buf1[10];
00260         char buf2[10];
00261         char buf3[10];
00262         char buf4[10];
00263         
00264         sprintf(buf1,"%3.0f",err);
00265         sprintf(buf2,"%3.0f",med);
00266         sprintf(buf3,"%i",spnum);
00267         sprintf(buf4,"%3.0f",pid);
00268         strcpy (bufferenvio,str1);
00269         strcat (bufferenvio,";");
00270         strcat (bufferenvio,buf1);
00271         strcat (bufferenvio,";");
00272         strcat (bufferenvio,buf2);
00273         strcat (bufferenvio,";");
00274         strcat (bufferenvio,buf3);
00275         strcat (bufferenvio,";");
00276         strcat (bufferenvio,buf4);
00277         strcat (bufferenvio,";");
00278         pc.printf("Datos a enviar= %s\r\n",bufferenvio);
00279         BLUETOOTH.printf("%s\r\n", bufferenvio);
00280         
00281         // se envía algo del tipo SEND;XXX.XX;XXX.XX;XXX;XXX.XX
00282         // al usar SPLIT por ; se tienen 5 elementos
00283 }
00284 }