Andrés López
PID programable a traves de un modulo bluetooth
Dependencies: BufferedSerial QEI TextLCD mbed
Diff: main.cpp
- Revision:
- 0:acd6b856d63c
--- /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;
+}