Procesadores, 2016 - II. Tarea #1. Oswaldo Andrés Giraldo Giraldo, Héctor Andrés Hoyos Ceballos, Jose Fernando Montoya Vargas, María Fernanda Villa Tamayo. Programa para establecer la comunicación con una aplicación en App Inventor por medio de Bluetooth.

Dependencies:   TextLCD mbed

Fork of TAREA_1_PID_BLUETOOTH by joshema 216

Revision:
0:24c2d41ff63f
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 16 02:14:33 2016 +0000
@@ -0,0 +1,419 @@
+// Programa para ejecutar un control PID que lee las constantes del controlador desde la UART,
+// que son enviadas por Bluetooth desde un proyecto de App Inventor ejecutado en un dispositivo
+// Android como cadena de caracteres. Cuando se inicia el proceso de control se envían los 
+// parámetros del mismo por Bluetooth hacia el dispositivo Android, el proyecto en App Inventor
+// permite recibir la cadena de parámetros y separarlos para asignarlos a su respectivo indicador.
+
+// Oswaldo Andrés Giraldo Giraldo - C.C.: 1152458465
+// Héctor Andrés Hoyos Ceballos - C.C.: 1039466317
+// Jose Fernando Montoya Vargas - C.C.: 1039468676
+// María Fernanda Villa Tamayo - C.C.: 1152457490
+
+#include "mbed.h"
+#include "TextLCD.h"
+
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // RS, E, D4 - D7
+Serial andr(USBTX, USBRX); // TX, RX
+Serial device(PTE0, PTE1);  // TX, RX
+
+AnalogIn y(PTB3); // Entrada análoga
+AnalogOut u(PTE30); // Salida análoga
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+// Códigos movimiento del cursor.
+//int C1=0x0E; // Sólo muestra el cursor.
+int C2=0x18; // Desplaza hacia la izquierda.
+int C3=0x1A; // Desplaza hacia la derecha.
+int C4=0x0C; // Quita cursor bajo.
+int C1=0x0F;
+
+float pid,o,ai,ad,ap,med,err,medr;
+float err_v;
+int spnum = 0, kinum = 0,kpnum = 0, kdnum = 0;
+char outmed[16], outmedn[16], outpid[16], outpidn[16], outspnum[16], outspnumn[16], outerr[16], outerrn[16], varout[40];
+
+Timer t;
+char buffer[4], resp[4], ind;
+int count, i;
+int k = 0, j = 0, num = 0;
+
+// Reverses a string 'str' of length 'len'
+// driver program to test above funtion.
+void reverse(char *str, int len)
+{
+    int i=0, j=len-1, temp;
+    while (i<j)
+    {
+        temp = str[i];
+        str[i] = str[j];
+        str[j] = temp;
+        i++; j--;
+    }
+}
+ 
+ // Converts a given integer x to string str[].  d is the number
+ // of digits required in output. If d is more than the number
+ // of digits in x, then 0s are added at the beginning.
+int intToStr(int x, char str[], int d)
+{
+    int i = 0;
+    while (x)
+    {
+        str[i++] = (x%10) + '0';
+        x = x/10;
+    }
+ 
+    // If number of digits required is more, then
+    // add 0s at the beginning
+    while (i < d)
+        str[i++] = '0';
+ 
+    reverse(str, i);
+    str[i] = '\0';
+    return i;
+}
+
+// Converts a floating point number to string.
+void ftoa(float n, char *res, int afterpoint)
+{
+    // Extract integer part
+    int ipart = (int)n;
+ 
+    // Extract floating part
+    float fpart = n - (float)ipart;
+ 
+    // convert integer part to string
+    int i = intToStr(ipart, res, 0);
+ 
+    // check for display option after point
+    if (afterpoint != 0)
+    {
+        res[i] = '.';  // add dot
+ 
+        // Get the value of fraction part upto given no.
+        // of points after dot. The third parameter is needed
+        // to handle cases like 233.007
+        float fp=10;
+        fpart =fpart * pow(fp,afterpoint);
+        
+        intToStr((int)fpart, res + i + 1, afterpoint);
+    }
+}
+
+// Lee la cadena de caracteres que llegan a la UART.
+int readBuffer(char *buffer,int count)
+{
+    int i=0; 
+    t.start();  // start timer
+    while(1) {
+        while (device.readable()) {
+            char c = device.getc();
+            if (c == '\r' || c == '\n') c = '$';
+            buffer[i++] = c;
+            if(i > count)break;
+        }
+        if(i > count)break;
+        if(t.read() > 1) {
+            t.stop();
+            t.reset();
+            break;
+        }
+    }
+    return 0;
+}
+
+// Limpia una cadena de caracteres.
+void cleanBuffer(char *buffer, int count)
+{
+    for(int i=0; i < count; i++) {
+        buffer[i] = '\0';
+    }
+}
+
+int main(void) {
+    device.baud(9600);// Configura los baudios de la FRDMKL25Z en 9600.
+    device.format(8,Serial::None,1); // Configura el formato de los datos de la UART.   
+    lcd.locate(1,1); // Se posiciona en col 1 fil 1.
+    lcd.printf("**Control PID**");
+    wait(2);
+
+    lop0: // CICLO DE ASIGNACIÓN DE VARIABLES.
+        lcd.cls(); // Borrar Pantalla.
+        lcd.writeCommand(C1);// Escribimos un comando segun el manual del modulo LCD.
+        lcd.locate(8,0);
+        lcd.printf("Kp=%d",kpnum); // Constante proporcional.
+        lcd.locate(0,1);
+        lcd.printf("Ki=%d",kinum); // Constante integral.
+        lcd.locate(8,1);
+        lcd.printf("Kd=%d",kdnum); // Constante derivativa.
+        lcd.locate(0,0);
+        lcd.printf("Sp=%d",spnum); // Set point (punto de ajuste)
+    
+        while(1){                    
+            if(device.readable()){ // Si llega algún paquete por la UART.
+                readBuffer(buffer,4); // Leer la cadena de caracteres.
+                led1 =!led1;
+                //andr.putc(device.getc());
+                andr.printf("%s\r\n",buffer);
+                ind = buffer[0]; // Las cadenas que llegan tienen un indicador en la primera posición.
+            
+                if (ind == '4') { // Si llega una cadena con el indicador '4' (Sp).
+                    resp[0] = buffer[1]; // Toma el valor de la constante ingresada.
+                    resp[1] = buffer[2];
+                    resp[2] = buffer[3];  
+                    spnum = strtod(resp,NULL); // Asigna la cadena a un entero.
+                    //andr.printf("%s\r\n",resp);
+                    lcd.locate(3,0);
+                    lcd.printf("    ");
+                    lcd.locate(3,0);
+                    lcd.printf("%d", spnum); // Imprime la referencia.
+                    led1 =!led1;
+                }
+                
+                if (ind == '1') { // Si llega una cadena con el indicador '1' (Kp).
+                    resp[0] = buffer[1]; // Toma el valor de la constante ingresada.
+                    resp[1] = buffer[2];
+                    resp[2] = buffer[3];  
+                    kpnum = strtod(resp,NULL); // Asigna la cadena a un entero.
+                    //pc.printf("%s\r\n",resp);
+                    lcd.locate(11,0);
+                    lcd.printf("    ");
+                    lcd.locate(11,0);
+                    lcd.printf("%d", kpnum); // Imprime Kp.
+                    led1 =!led1;
+                }
+                
+                if (ind == '2') { // Si llega una cadena con el indicador '2' (Ki).
+                    resp[0] = buffer[1]; // Toma el valor de la constante ingresada.
+                    resp[1] = buffer[2];
+                    resp[2] = buffer[3];  
+                    kinum = strtod(resp,NULL); // Asigna la cadena a un entero.
+                    //pc.printf("%s\r\n",resp);                
+                    lcd.locate(3,1);
+                    lcd.printf("    ");
+                    lcd.locate(3,1);
+                    lcd.printf("%d", kinum); //Imprime Ki.
+                    led1 =!led1;
+                }
+                
+                if (ind == '3') { // Si llega una cadena con el indicador '3' (Kd).
+                    resp[0] = buffer[1]; // Toma el valor de la constante ingresada.
+                    resp[1] = buffer[2];
+                    resp[2] = buffer[3];  
+                    kdnum = strtod(resp,NULL); // Asigna la cadena a un entero.
+                    //pc.printf("%s\r\n",resp);
+                    lcd.locate(11,1);
+                    lcd.printf("    ");
+                    lcd.locate(11,1);
+                    lcd.printf("%d", kdnum); // Imprime Kd.
+                    led1 =!led1;
+                }
+                
+                if (ind == '5') { // Si llega una cadena con el indicador '5' (Borrar).
+                    led1 =!led1;
+                    led2 =!led2;
+                    led3 =!led3;
+                    kpnum = 0; // Borra las constantes del controlador.
+                    kinum = 0;
+                    kdnum = 0;
+                    spnum = 0;
+                    lcd.cls(); // Borrar Pantalla.
+                    lcd.locate(1,1); // Se posiciona en col 1 fil 1.
+                    lcd.printf("DATOS BORRADOS");
+                    wait(2);
+                    lcd.cls(); // Borrar Pantalla.
+                    lcd.writeCommand(C1);// Escribimos un comando segun el manual del modulo LCD.
+                    lcd.locate(8,0);
+                    lcd.printf("Kp=%d",kpnum);
+                    lcd.locate(0,1);
+                    lcd.printf("Ki=%d",kinum);
+                    lcd.locate(8,1);
+                    lcd.printf("Kd=%d",kdnum);
+                    lcd.locate(0,0);
+                    lcd.printf("Sp=%d",spnum);
+                    led2 =!led2;
+                    led3 =!led3;
+                }
+                
+                if (ind == '7') { // Si llega una cadena con el indicador '7' (Detener).
+                    led1 =!led1; // No hacer nada, pues el controlador no se ha ejecutado.
+                }
+                
+                if (ind == '6') { // Si llega una cadena con el indicador '6' (Iniciar).
+                    led1 =!led1;
+                    led3 =!led3;
+                    
+                    // Transición.
+                    lcd.writeCommand(C4);//Escribimos un comando segun el manual del modulo LCD para quitar cursor bajo.
+                    lcd.cls(); // Borra la pantalla.
+                    lcd.locate(1,1); // Se posiciona en col 1 fil 1.
+                    lcd.printf("DATOS GUARDADOS");
+                    wait(1);
+                    lcd.cls();
+                    lcd.locate(1,1); // Se posiciona en col 1 fil 1.
+                    lcd.printf("INICIO DEL PID");
+                    wait(2);
+                    
+                    // Se imprimen los parches del control.
+                    lcd.cls();
+                    lcd.printf("Er=%3.0f",err);
+                    lcd.locate(8,0);
+                    lcd.printf("Me=%3.0f",med);
+                    lcd.locate(0,1);
+                    lcd.printf("Sp=%3.0f",spnum);
+                    lcd.locate(8,1);
+                    lcd.printf("Co=%3.0f",pid);
+                    wait(1);
+                    
+                    // CICLO PRINCIPAL CONTROLADOR PID.
+                    lop1:
+                        med = y.read()*999; // Se toma la medida actual por el puerto analógico.
+                        //andr.printf("%f", med);
+                        err = (spnum-med);  // Se calcula el error.
+                        //andr.printf("%f", err);
+                        ap = kpnum*err*0.01f; // Se calcula la acción proporcinal.
+                        ai =(kinum*err*0.01f)+ai; // Cálculo de la integral del error.
+                        ad = kdnum*(err-err_v)*0.01f; // Cálculo de la acción derivativa.
+                        pid = (ap+ai+ad); // Se actualiza el valor del la acción de control.     
+                        //andr.printf("%f", pid);
+                                            
+                        // Se verifica que pid sea positivo.
+                        if(pid<=0) {
+                            pid=0;
+                        }
+                    
+                        // Se verifica que pid sea menor o igual la valor máximo.
+                        if (pid > 999) {
+                            pid=999;
+                        }
+                        
+                        // Se envían los datos del control como caracteres.
+                        cleanBuffer(varout, 40);
+                        cleanBuffer(outerr, 16);
+                        cleanBuffer(outmed, 16);
+                        cleanBuffer(outpid, 16);
+                        cleanBuffer(outspnum, 16);
+                        
+                        ftoa(spnum, outspnum, 4); // Se convierte el Set point a caracter.
+                        
+                        if (err < 0 && err > -1){ // Se convierte el Error a caracter.
+                            err = (-1)*err;
+                            strcat(outerr, "0");
+                            ftoa(err, outerrn, 4);
+                            strcat(outerr, outerrn);
+                        }
+                        else if (err <= -1){
+                            err = (-1)*err;
+                            ftoa(err, outerr, 4);
+                        }
+                        else if (err > 0 && err < 1){
+                            strcat(outerr, "0");
+                            ftoa(err, outerrn, 4);
+                            strcat(outerr, outerrn);
+                        }
+                        else{
+                            ftoa(err, outerr, 4);
+                        }
+                        
+                        if (med < 1){ // Se convierte la Medida a caracter.
+                            strcat(outmed, "0");
+                            ftoa(med, outmedn, 4);
+                            strcat(outmed, outmedn);
+                        }
+                        else{
+                            ftoa(med, outmed, 4);
+                        }
+                        
+                        if (pid < 1){ // Se convierte la Acción de control a caracter.
+                            strcat(outpid, "0");
+                            ftoa(pid, outpidn, 4);
+                            strcat(outpid, outpidn);
+                        }
+                        else{
+                            ftoa(pid, outpid, 4);
+                        }
+                            
+                        strcat(varout, " /"); // Se prepara la cadena a ser enviada, se separa cada constante del control por "/".
+                        strcat(varout, outspnum);
+                        strcat(varout, "/");
+                        strcat(varout, outerr);
+                        strcat(varout, "/");
+                        strcat(varout, outmed);
+                        strcat(varout, "/");
+                        strcat(varout, outpid);
+                        
+                        device.printf(varout);     
+                        andr.printf("%s\n", varout);
+                    
+                        // Se muestran las variables en la LCD.
+                        lcd.locate(3,0);
+                        lcd.printf("    ");
+                        lcd.locate(3,0);
+                        lcd.printf("%3.0f",err);
+                        lcd.locate(11,0);
+                        lcd.printf("   ");
+                        lcd.locate(11,0);
+                        lcd.printf("%3.0f",med);
+                        lcd.locate(3,1);
+                        lcd.printf("   ");
+                        lcd.locate(3,1);
+                        lcd.printf("%3.0d",spnum);
+                        lcd.locate(11,1);
+                        lcd.printf("   ");
+                        lcd.locate(11,1);
+                        lcd.printf("%3.0f",pid);
+                    
+                        // Normalización de la salida.
+                        // Se actualizan las variables.
+                        err_v = err;
+                        o = pid/999;
+                        u.write(o); // Se envía el valor pid a puerto analogico de salida (D/A).
+                        
+                        // Se repite el ciclo.
+                        wait_ms(300);
+                        
+                        if(device.readable()){ // Si llega alguna cadena de caracteresp por la UART, principalmente para detener el controlador.
+                            //andr.putc(device.getc());
+                            readBuffer(buffer,4);
+                            //andr.printf("%s\r\n",buffer);
+                            ind = buffer[0];
+                            
+                            if (ind == '7'){ // Si llega una cadena con el indicador '7' (Dentener).
+                                andr.printf("%s", ind);
+                                pid = 0; // Regresa las variables del proceso de control a cero.
+                                ai = 0;
+                                ad = 0;
+                                ap = 0;
+                                med = 0;
+                                err = 0;
+                                err_v = 0;
+                                
+                                lcd.cls(); // Borrar Pantalla
+                                lcd.locate(1,1); //se posiciona en col 1 fil 1
+                                lcd.printf("PID DETENIDO");
+                                wait(2);
+                                lcd.cls(); // Borrar Pantalla
+                                lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
+                                led3 =!led3;
+                                
+                                lcd.locate(8,0);
+                                lcd.printf("Kp=%d",kpnum);
+                                lcd.locate(0,1);
+                                lcd.printf("Ki=%d",kinum);
+                                lcd.locate(8,1);
+                                lcd.printf("Kd=%d",kdnum);
+                                lcd.locate(0,0);
+                                lcd.printf("Sp=%d",spnum);
+                            }
+                            cleanBuffer(buffer,4);
+                            goto lop0; // Si en efecto se detiene el PID vuelve al ciclo de asignación de variables.
+                        }
+                        goto lop1; // Si no se detiene el PID entonces vuelve al ciclo de control.
+                }
+                cleanBuffer(buffer,4);
+            }
+            //andr.putc(device.getc());  
+        }
+}
\ No newline at end of file