PID programable a traves de un modulo bluetooth

Dependencies:   BufferedSerial QEI TextLCD mbed

Files at this revision

API Documentation at this revision

Comitter:
andJdmat
Date:
Wed Nov 30 02:00:39 2016 +0000
Commit message:
PID programable a traves de bluetooth;

Changed in this revision

BufferedSerial.lib Show annotated file Show diff for this revision Revisions of this file
QEI.lib Show annotated file Show diff for this revision Revisions of this file
TextLCD.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r acd6b856d63c BufferedSerial.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BufferedSerial.lib	Wed Nov 30 02:00:39 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/sam_grove/code/BufferedSerial/#a0d37088b405
diff -r 000000000000 -r acd6b856d63c QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Nov 30 02:00:39 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
diff -r 000000000000 -r acd6b856d63c TextLCD.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Wed Nov 30 02:00:39 2016 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/avallejopo/code/TextLCD/#aba8ab3dde9d
diff -r 000000000000 -r acd6b856d63c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Nov 30 02:00:39 2016 +0000
@@ -0,0 +1,614 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "TextLCD.h"
+#include "iostream"
+#include "stdio.h"
+#include "string"
+#include "stdlib.h"
+#include <BufferedSerial.h>
+
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5, TextLCD::LCD20x4); // rs, e, d4-d7 Teclado
+//asignamos el puerto a cada interruptor
+QEI encoder (PTA13, PTD5, NC, 624, QEI::X4_ENCODING);
+Serial GSM(PTE0,PTE1);
+AnalogIn y(PTB3);//entrada analoga
+AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer
+//si se ignora esto se arruina la FRDMKL25Z
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+ 
+DigitalIn button3(PTC16);//cambia ingreso de  los 4 parametros
+DigitalIn button4(PTC17);//termina y consolida valores de 4 parametros y sale del loop
+
+Serial pc(USBTX, USBRX); // tx, rx
+Serial device(PTE0, PTE1);  // tx, rx
+//device.baud(115200); 
+ 
+ 
+//codigos movimiento del curzor
+ 
+//int C1=0x0E; // solo muestra el curzor
+int C2=0x18; // desplaza izquierda
+int C3=0x1A; // desplaza derecha
+int C4=0x0C; // quito cursor bajo
+ 
+int C1=0x0F;
+int cambio=0, diferencia=0;
+// se cambio de float a entero
+
+float pid,o,ai,ad,ap,med,err;
+float err_v;
+
+// Valores de k y de j
+int j=0;
+int k=0;
+
+// fin del cambio
+int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1;
+char buffer[128];
+char buffer2[128];
+char salidas[32];
+char err_s[3];
+char spnum_s[3];
+char med_s[3];
+char co_s[3];
+Timer t;
+int l;
+
+
+// Inicio de modificaciones, en esta parte se agregaron funciones que permiten imprimir datos en la lcd que envia
+// el modulo bluetooth, el codigo fue depurado usando la comunicacion serial. 
+char* itoa(int value, char* result, int base)     // Funcion para convertir enteros a cadenas de texto, fue importada de un foro                                                                                                     
+{                                                                                                                                                        
+    // check that the base if valid                                                                                                                      
+    if ( base < 2 || base > 36 ) {                                                                                                                       
+        *result = '\0';                                                                                                                                  
+        return result;                                                                                                                                   
+    }                                                                                                                                                    
+ 
+    char* ptr = result, *ptr1 = result, tmp_char;                                                                                                        
+    int tmp_value;                                                                                                                                       
+ 
+    do {                                                                                                                                                 
+        tmp_value = value;                                                                                                                               
+        value /= base;                                                                                                                                   
+        *ptr++ = "zyxwvutsrqponmlkjihgfedcba9876543210123456789abcdefghijklmnopqrstuvwxyz"[35 + (tmp_value - value * base)];                             
+    } while ( value );                                                                                                                                   
+ 
+    // Apply negative sign                                                                                                                               
+    if ( tmp_value < 0 )                                                                                                                                 
+    *ptr++ = '-';                                                                                                                                    
+    *ptr-- = '\0';                                                                                                                                       
+ 
+    while ( ptr1 < ptr ) {                                                                                                                               
+    tmp_char = *ptr;                                                                                                                                 
+    *ptr-- = *ptr1;                                                                                                                                  
+    *ptr1++ = tmp_char;                                                                                                                              
+    }                                                                                                                                                    
+ 
+    return result;                                                                                                                                       
+}
+ 
+
+int readBuffer(char *buffer,int count)   //esta funcion lee un bufer de datos
+{
+    int i=0; 
+    t.start();    //CUENTA EL TIEMPO DE CONEXION E INICIA
+    while(1) {
+      while (device.readable()) {
+            char c = device.getc();
+            //if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracxter inserta $
+            buffer[i++ - 1] = c;//mete al bufer el caracter leido
+            if(i > count)break;//sale del loop si ya detecto terminacion
+        }
+        if(i > count)break;
+        if(t.read() > 1) {  //MAS DE UN SEGUNDO DE ESPERA SE SALE Y REINICA EL RELOJ Y SE SALE
+            t.stop();
+            t.reset();
+            break;
+        }
+    }   
+     return 0;
+}
+ 
+void cleanBuffer(char *buffer, int count)  //esta funcion limpia el bufer
+{
+    for(int i=0; i < count; i++) {
+        buffer[i] = '\0';
+    }
+}
+
+int tamano(char *buffer)
+{
+int tam = 0;
+    while (buffer[tam] != '\0')
+    {
+        tam++;    
+    }
+    return tam;
+}
+
+void verificacion(char *buffer) // Esta funcion fue creada para esta aplicacion especifica, permite verificar si los datos que entraron a la freescale
+// son los esperados y adicionalmente crea un nuevo buffer con los datos para imprimir.
+{
+    int l = tamano(buffer);
+    buffer2[0] = '-';
+    if (l < 16)
+    {
+        if(buffer[0]=='\0')
+        {
+            for (int i = 0; i < 16; i++)
+                {
+                    buffer2[i] = '0';
+                }
+        }
+        else 
+        {         
+            int k = 0;
+            int j = 0;
+            for(int i=0; i < 15; i++) 
+            {
+                if(j == 1)
+                {
+                    if(buffer[i+1]=='-')
+                    {
+                        j = 0;
+                    }
+                    else if (buffer[i+2]=='-')
+                    {
+                        j = 0;
+                        i = i+1;
+                    }
+                    else if (buffer[i+3]=='-')
+                    {
+                        j = 0;
+                        i = i+2;
+                    }
+                    else if(buffer[i+4]=='-')
+                    {
+                        j = 0;
+                        i = i+3;                 
+                    }
+                }
+
+                else
+                {       
+                    if(buffer[i+1]=='-')
+                    {
+                        buffer2[k + 1] = '0';
+                        buffer2[k + 2] = '0';
+                        buffer2[k + 3] = '0';
+                        buffer2[k + 4] = '-';
+                        k = k + 4;
+                        j = 1;
+                    }
+                else if (buffer[i+2]=='-')
+                    {
+                        buffer2[k + 1] = '0';
+                        buffer2[k + 2] = '0';
+                        buffer2[k + 3] = buffer[i+1];
+                        buffer2[k + 4] = '-';
+                        k = k + 4;
+                        j = 1;
+                    }
+                else if (buffer[i+3]=='-')
+                    {
+                        buffer2[k + 1] = '0';
+                        buffer2[k + 2] = buffer[i+1];
+                        buffer2[k + 3] = buffer[i+2];
+                        buffer2[k + 4] = '-'; 
+                        k = k+4;
+                        j = 1;
+                    }
+                else if(buffer[i+4]=='-')
+                    {
+                        buffer2[k + 1] = buffer[i+1];
+                        buffer2[k + 2] = buffer[i+2];
+                        buffer2[k + 3] = buffer[i+3];
+                        buffer2[k + 4] = '-';
+                        k = k+4;
+                        j =1;                 
+                    }
+                }
+            }
+        }  
+    }
+    else
+    {
+        for (int i = 0; i < 16; i++)
+        {
+            buffer2[i] = buffer[i];
+        }
+    }
+}
+// Fin de la modificacion para la impresion por bluetooth
+int main()
+{
+    lcd.locate(0,1);
+    lcd.printf("**Control PID**");
+    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);
+ 
+    while(1)
+    {
+        //lcd.locate(8,0);
+        //lcd.printf("Kp=%d",encoder.getPulses());
+        //wait(.5);
+        
+        if(device.readable()) // Se comprueba si ha datos para leer y se imprimen.
+            {   
+        
+        cleanBuffer(buffer2,128);
+        cleanBuffer(buffer,128);
+        readBuffer(buffer,128);
+        if (buffer[0] == 'p'){break;}
+        pc.printf("buffer= %s\n\r ",buffer);  //imprime el bufer
+        verificacion(buffer);
+        pc.printf("buffer2= %s\n\r ",buffer2);  //imprime el buffer reconstruido
+        pc.printf("tamano buffer= %d \n\r",l);  //imprime el tamaño
+        pc.printf("Sp= %c %c %c\n\r ",buffer2[1],buffer2[2],buffer2[3]);
+        pc.printf("Kp= %c %c %c\n\r ",buffer2[5],buffer2[6],buffer2[7]);
+        pc.printf("Ki= %c %c %c\n\r ",buffer2[9],buffer2[10],buffer2[11]);
+        pc.printf("Kd= %c %c %c\n\r ",buffer2[13],buffer2[14],buffer2[15]);
+        spnum = 1*(buffer2[3]-48)+ 10*(buffer2[2]-48)+ 100*(buffer2[1]-48);
+        
+        lcd.locate(3,0);
+        lcd.printf("     ");
+        lcd.locate(3,0);
+        lcd.printf("%d", spnum);
+        kpnum = 1*(buffer2[7]-48)+ 10*(buffer2[6]-48)+ 100*(buffer2[5]-48);
+        lcd.locate(11,0);
+        lcd.printf("     ");
+        lcd.locate(11,0);
+        lcd.printf("%d", kpnum);
+        kinum = 1*(buffer2[11]-48)+ 10*(buffer2[10]-48)+ 100*(buffer2[9]-48);
+        lcd.locate(3,1);
+        lcd.printf("     ");
+        lcd.locate(3,1);
+        lcd.printf("%d", kinum);
+        kdnum = 1*(buffer2[15]-48)+ 10*(buffer2[14]-48)+ 100*(buffer2[13]-48);
+        lcd.locate(11,1);
+        lcd.printf("     ");
+        lcd.locate(11,1);
+        lcd.printf("%d", kdnum);
+        cleanBuffer(buffer2,128);
+        cleanBuffer(buffer,128);          
+            }
+        else{
+        
+        diferencia=encoder.getPulses()-cambio;
+        cambio=encoder.getPulses();
+ 
+        if (diferencia==0)
+        {
+            //nada
+        }
+        else if(diferencia>0)
+        {
+            if(pos==1)
+            {
+                if(spnum+diferencia>=999)
+                {
+                    spnum=999;
+                    lcd.locate(3,0);
+                    lcd.printf("    ");
+                    lcd.locate(3,0);
+                    lcd.printf("%d", spnum);
+                }
+                else
+                {
+                    spnum+=diferencia;
+                    lcd.locate(3,0);
+                    lcd.printf("%d", spnum);
+                }
+            }
+            else if(pos==2)
+            {
+                if(kpnum+diferencia>=999)
+                {
+                    kpnum=999;
+                    lcd.locate(11,0);
+                    lcd.printf("    ");
+                    lcd.locate(11,0);
+                    lcd.printf("%d", kpnum);
+                }
+                else
+                {
+                    kpnum+=diferencia;
+                    lcd.locate(11,0);
+                    lcd.printf("%d", kpnum);
+                }
+            }
+            else if(pos==3)
+            {
+                if(kinum+diferencia>=999)
+                {
+                    kinum=999;
+                    lcd.locate(3,1);
+                    lcd.printf("    ");
+                    lcd.locate(3,1);
+                    lcd.printf("%d", kinum);
+                }
+                else
+                {
+                    kinum+=diferencia;
+                    lcd.locate(3,1);
+                    lcd.printf("%d", kinum);
+                }
+            }
+            else if(pos==4)
+            {
+                if(kdnum+diferencia>=999)
+                {
+                    kdnum=999;
+                    lcd.locate(11,1);
+                    lcd.printf("    ");
+                    lcd.locate(11,1);
+                    lcd.printf("%d", kdnum);
+                }
+                else
+                {
+                    kdnum+=diferencia;
+                    lcd.locate(11,1);
+                    lcd.printf("%d", kdnum);
+                }
+            }
+        }
+        
+        else if(diferencia<0)
+        {
+            if(pos==1)
+            {
+                if(spnum+diferencia<0)
+                {
+                    //No ocurre nada
+                }
+                else
+                {
+                    spnum+=diferencia;
+                    lcd.locate(3,0);
+                    lcd.printf("    ");
+                    lcd.locate(3,0);
+                    lcd.printf("%d", spnum);
+                }
+            }
+            else if(pos==2)
+            {
+                if(kpnum+diferencia<0)
+                {
+                    //No ocurre nada
+                }
+                else
+                {
+                    kpnum+=diferencia;
+                    lcd.locate(11,0);
+                    lcd.printf("    ");
+                    lcd.locate(11,0);
+                    lcd.printf("%d", kpnum);
+                }
+            }
+            else if(pos==3)
+            {
+                if(kinum+diferencia<0)
+                {
+                    //No ocurre nada
+                }
+                else
+                {
+                    kinum+=diferencia;
+                    lcd.locate(3,1);
+                    lcd.printf("    ");
+                    lcd.locate(3,1);
+                    lcd.printf("%d", kinum);
+                }
+            }
+            else if(pos==4)
+            {
+                if(kdnum+diferencia<0)
+                {
+                    //No ocurre nada
+                }
+                else
+                {
+                    kdnum+=diferencia;
+                    lcd.locate(11,1);
+                    lcd.printf("    ");
+                    lcd.locate(11,1);
+                    lcd.printf("%d", kdnum);
+                }
+            }
+         }   
+    } 
+        if (!button3)  //cambia la posicion de ingreso de parametros
+        {
+            led3 =!led3;
+            if(pos==4)
+            {
+                pos=1;
+                lcd.locate(3,0);
+                lcd.printf("%d", spnum);
+            }
+            else if (pos==1)
+            {
+                pos++;
+                lcd.locate(11,0);
+                lcd.printf("%d", kpnum);
+            }
+            else if(pos==2)
+            {
+                pos++;
+                lcd.locate(3,1);
+                lcd.printf("%d", kinum);
+            }
+            else if(pos==3)
+            {
+                pos++;
+                lcd.locate(11,1);
+                lcd.printf("%d", kdnum);
+            }
+            wait(0.25);
+ 
+        }
+ 
+        if (!button4)
+        {
+            break;     //sale del bucle si pisan suiche4
+        }
+        wait(0.1);        
+    }
+ 
+ 
+//Transicion
+    lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
+    lcd.cls(); //borra la pantalla
+    lcd.printf("   GUARDADOS!");
+    wait(1);
+    lcd.cls();
+    lcd.printf(" INICIA EL PID");
+    wait(1);
+// 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;
+        err = (spnum-med);  //se calcula el error
+        ap = kpnum*err*0.01f;     //se calcula la accion proporcinal
+        ai =(kinum*err*0.01f)+ai;    //calculo de la integral del error
+        ad = kdnum*(err-err_v)*0.01f; //calculo de la accion derivativa
+        pid = (ap+ai+ad);
+        // se verifica que pid sea positivo **************************************
+        if(pid<=0)
+        {
+            pid=0;
+        }
+ 
+        // se verifica que pid sea menor o igual la valor maximo *****************
+        if (pid > 999)
+        {
+            pid=999;
+        }
+ 
+       
+        //se muestran las variables******************************************
+            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("%d",spnum);
+            lcd.locate(11,1);
+            lcd.printf("   ");
+            lcd.locate(11,1);
+            lcd.printf("%3.0f",pid);
+
+        // Modificacion para enviar los datos por bluetooth
+        
+        
+        cleanBuffer(err_s,3);
+        cleanBuffer(spnum_s,3); 
+        cleanBuffer(co_s,3);   
+        cleanBuffer(med_s,3); 
+        cleanBuffer(salidas,17);
+        //pc.printf("error= %f\n\r ",err);
+                
+        char *r = ".";
+        
+        
+        itoa(int(err),err_s,10);
+        //pc.printf("error= %s\n\r ",err_s);
+        strcpy(salidas, err_s);
+        strcat(salidas, r); 
+        
+        itoa(int(spnum),spnum_s,10);
+        //pc.printf("sp= %s\n\r ",spnum_s);
+        strcat(salidas, spnum_s);
+        strcat(salidas, r); 
+        
+        itoa(int(pid),co_s,10);
+        //pc.printf("CO= %s\n\r ",co_s);
+        strcat(salidas, co_s);
+        strcat(salidas, r); 
+        
+        itoa(int(med),med_s,10);
+        //pc.printf("med= %s\n\r ",med_s);
+        strcat(salidas, med_s);
+        strcat(salidas, r);   
+        salidas[17]='.';   
+        pc.printf("Salidas= %s\n\r ",salidas);
+        
+        
+        device.puts(salidas);
+        
+        cleanBuffer(err_s,3);
+        cleanBuffer(spnum_s,3); 
+        cleanBuffer(co_s,3);   
+        cleanBuffer(med_s,3); 
+        cleanBuffer(salidas,17);
+        /*
+       if(err<256)
+       {           //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)    
+           device.putc(0);     //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
+           device.putc(err);     //luego la cifra menos significativa
+       }
+       if(err>255)
+       {          //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma   
+           j=err/256;       //calculo la cifra mas significativa
+           k=err-j*256;     //calculo la cifra menos significativa
+           device.putc(j);   //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
+           device.putc(k);   //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
+       }   
+        
+       if(med<256)
+       
+       {           //debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)    
+           device.putc(0);     //si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
+           device.putc(spnum);     //luego la cifra menos significativa
+       }
+       
+       if(med>255)
+        {          //pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma   
+           j = 0;
+           k = 0;
+           j=med/256;       //calculo la cifra mas significativa
+           k=med-j*256;     //calculo la cifra menos significativa
+           device.putc(j);   //las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
+           device.putc(k);   //mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
+        }  
+        */ // Fin de la modificacion
+            
+        //Normalizacion de la salida
+        // se actualizan las variables *******************************************
+        err_v = err;
+        o = pid/999;
+        u.write(o);
+        //  se envia el valor pid a puerto analogico de salida (D/A) **************
+        
+        //  se repite el ciclo
+        wait_ms(300);
+        goto lop1;
+}
diff -r 000000000000 -r acd6b856d63c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Nov 30 02:00:39 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3
\ No newline at end of file