![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
PID programable a traves de un modulo bluetooth
Dependencies: BufferedSerial QEI TextLCD mbed
main.cpp
- Committer:
- andJdmat
- Date:
- 2016-11-30
- Revision:
- 0:acd6b856d63c
File content as of revision 0:acd6b856d63c:
#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; }