deiwid ricaurte
programa controlador pid https://drive.google.com/drive/u/0/folders/0B17IW4rNa3-UZDlEdlBNVzlWdGc
Dependencies: QEI TextLCD mbed
Diff: main.cpp
- Revision:
- 0:7f32c2a0d548
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Oct 26 14:36:17 2016 +0000
@@ -0,0 +1,531 @@
+#include "mbed.h"
+#include "stdio.h"
+#include "string.h"
+#include "TextLCD.h"
+#include "QEI.h"
+
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
+QEI encoder (PTA13, PTD5, NC, 624);
+AnalogIn y(PTB3);//entrada analoga
+AnalogOut u(PTE30);//salida analoga OJO solo se le pueden drenar 1.5mA en circuitos use un Buffer
+
+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 uart(PTE0,PTE1); //puertos del FRDM para el modem
+Serial pc(USBTX,USBRX); //puertos del PC
+
+char buffer[4];// TAMAÑO DEL BUFER
+Timer t,t1; //VALOR DEL TIEMPO
+int count;
+int contador;
+int i = 0;
+int c=0;
+int sp=0,ki=0,kp=0,kd=0;
+int C1=0x0F;
+int C4=0x0C;
+int j,k;
+float pid,o,ai,ad,ap,med=0.0,err=0.0;
+float err_v,med_v;
+//char r[]="";
+//char clave[]="abcdefghijklmnopqrst";
+
+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 (uart.readable()) {
+ char c = uart.getc();
+ if (c == '\r' || c == '\n') c = '$';//si se envia fin de linea o de caracxter inserta $
+ buffer[i++] = 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';
+ }
+}
+
+//
+void enviarvalores()
+{
+ wait(0.1);
+ if(sp<256)
+ {
+ uart.putc(sp);
+ uart.putc(0);
+
+ }
+ if(sp>255)
+ {
+ j=sp/256;
+ k=sp-j*256;
+ uart.putc(k);
+ uart.putc(j);
+
+ }
+ wait(0.001);
+ if(kp<256)
+ {
+ uart.putc(kp);
+ uart.putc(0);
+
+ }
+ if(kp>255)
+ {
+ j=kp/256;
+ k=kp-j*256;
+ uart.putc(k);
+ uart.putc(j);
+
+ }
+ wait(0.001);
+ if(ki<256)
+ {
+
+ uart.putc(ki);
+ uart.putc(0);
+
+ }
+ if(ki>255)
+ {
+ j=ki/256;
+ k=ki-j*256;
+ uart.putc(k);
+ uart.putc(j);
+
+ }
+ wait(0.001);
+ if(kd<256)
+ {
+ uart.putc(kd);
+ uart.putc(0);
+
+ }
+ if(sp>255)
+ {
+ j=kd/256;
+ k=kd-j*256;
+ uart.putc(k);
+ uart.putc(j);
+
+ }
+ err_v = err;
+ if(err_v<0)
+ {
+ err_v=err_v*(-1);
+ }
+
+ if(err_v<256)
+ {
+ uart.putc(err_v);
+ uart.putc(0);
+
+ }
+
+ if(err_v>255)
+ {
+ j=err_v/256;
+ k=err_v-j*256;
+ uart.putc(k);
+ uart.putc(j);
+
+ }
+ //wait(0.1);
+
+ med_v = med;
+ //wait(0.1);
+ if(med_v<256)
+ {
+ uart.putc(med_v);
+ uart.putc(0);
+
+ }
+
+ if(med_v>255)
+ {
+ j=med_v/256;
+ k=med_v-j*256;
+ uart.putc(k);
+ uart.putc(j);
+
+ }
+
+}
+//
+void encoderdatos()
+{
+
+ int pos=1;
+ int cambio=0, diferencia=0;
+
+ while(1)
+ {
+
+ diferencia=encoder.getPulses()-cambio;
+ cambio=encoder.getPulses();
+ if (diferencia==0)
+ {
+ //nada
+ }
+ else if(diferencia>0)
+ {
+ if(pos==1)
+ {
+
+ if(sp+diferencia>=999)
+ {
+ sp=999;
+ lcd.locate(3,0);
+ lcd.printf(" ");
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+ enviarvalores();
+ }
+ else
+ {
+ sp+=diferencia;
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+ enviarvalores();
+
+
+ }
+ }
+ else if(pos==2)
+ {
+ if(kp+diferencia>=999)
+ {
+ kp=999;
+ lcd.locate(11,0);
+ lcd.printf(" ");
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+ enviarvalores();
+ }
+ else
+ {
+ kp+=diferencia;
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+ enviarvalores();
+ }
+ }
+ else if(pos==3)
+ {
+ if(ki+diferencia>=999)
+ {
+ ki=999;
+ lcd.locate(3,1);
+ lcd.printf(" ");
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+ enviarvalores();
+ }
+ else
+ {
+ ki+=diferencia;
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+ enviarvalores();
+ }
+ }
+ else if(pos==4)
+ {
+ if(kd+diferencia>=999)
+ {
+ kd=999;
+ lcd.locate(11,1);
+ lcd.printf(" ");
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+ enviarvalores();
+ }
+ else
+ {
+ kd+=diferencia;
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+ enviarvalores();
+ }
+ }
+ }
+ else if(diferencia<0)
+ {
+ if(pos==1)
+ {
+ if(sp+diferencia<0)
+ {
+ //No ocurre nada
+ }
+ else
+ {
+ sp+=diferencia;
+ lcd.locate(3,0);
+ lcd.printf(" ");
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+ enviarvalores();
+ }
+ }
+ else if(pos==2)
+ {
+ if(kp+diferencia<0)
+ {
+ //No ocurre nada
+ }
+ else
+ {
+ kp+=diferencia;
+ lcd.locate(11,0);
+ lcd.printf(" ");
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+ enviarvalores();
+ }
+ }
+ else if(pos==3)
+ {
+ if(ki+diferencia<0)
+ {
+ //No ocurre nada
+ }
+ else
+ {
+ ki+=diferencia;
+ lcd.locate(3,1);
+ lcd.printf(" ");
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+ enviarvalores();
+ }
+ }
+ else if(pos==4)
+ {
+ if(kd+diferencia<0)
+ {
+ //No ocurre nada
+ }
+ else
+ {
+ kd+=diferencia;
+ lcd.locate(11,1);
+ lcd.printf(" ");
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+ enviarvalores();
+ }
+ }
+ }
+ if (!button3) //cambia la posicion de ingreso de parametros
+ {
+ led3 =!led3;
+ if(pos==4)
+ {
+ pos=1;
+ lcd.locate(3,0);
+ lcd.printf("%d", sp);
+ enviarvalores();
+ }
+ else if (pos==1)
+ {
+ pos++;
+ lcd.locate(11,0);
+ lcd.printf("%d", kp);
+ enviarvalores();
+ }
+ else if(pos==2)
+ {
+ pos++;
+ lcd.locate(3,1);
+ lcd.printf("%d", ki);
+ enviarvalores();
+ }
+ else if(pos==3)
+ {
+ pos++;
+ lcd.locate(11,1);
+ lcd.printf("%d", kd);
+ enviarvalores();
+ }
+ wait(0.25);
+
+ }
+
+
+ if (!button4)
+ {
+ break; //sale del bucle si pisan suiche4
+ }
+
+ wait(0.01);
+ }
+ //return spnum,kinum,kpnum,kdnum;
+
+}
+//
+
+int main(void)
+{
+
+ uart.baud(9600);
+ uart.format(8,Serial::None,1);
+ 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",kp);
+ lcd.locate(0,1);
+ lcd.printf("Ki=%d",ki);
+ lcd.locate(8,1);
+ lcd.printf("Kd=%d",kd);
+ lcd.locate(0,0);
+ lcd.printf("Sp=%d",sp);
+ wait(0.2);
+ encoderdatos();
+ 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",sp);
+ lcd.locate(8,1);
+ lcd.printf("Co=%3.0f",pid);
+ wait(1);
+
+
+ //ciclo control
+ while(1)
+ {
+ //Serial uart(PTE0,PTE1);
+ //Serial pc(USBTX,USBRX);
+ //uart.baud(9600);
+ //uart.format(8,Serial::None,1);
+ if (uart.readable())
+ {
+ readBuffer(buffer,4);
+
+ pc.printf("buffer= %s\n\r ",buffer);
+ if(buffer[0] == 'n')
+ {
+ med = y.read()*999;
+ err = (sp-med); //se calcula el error
+ ap = kp*err*0.01f; //se calcula la accion proporcinal
+ ai =(ki*err*0.01f)+ai; //calculo de la integral del error
+ ad = kd*(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",sp);
+ lcd.locate(11,1);
+ lcd.printf(" ");
+ lcd.locate(11,1);
+ lcd.printf("%3.0f",pid);
+
+
+
+ o = pid/999;
+ u.write(o);
+ // se envia el valor pid a puerto analogico de salida (D/A) **************
+
+ // se repite el ciclo
+ wait_ms(10);
+ }
+ else if(buffer[0] == 's')
+ {
+ int sp1=int(buffer[1]-48)*100+int(buffer[2]-48)*10+int(buffer[3]-48)*1;
+ if(sp1<1000)
+ {
+ sp=sp1;
+ }
+ wait(0.01);
+ }
+ else if(buffer[0] == 'p')
+ {
+ int kp1=int(buffer[1]-48)*100+int(buffer[2]-48)*10+int(buffer[3]-48)*1;
+ if(kp1<1000)
+ {
+ kp=kp1;
+ }
+ wait(0.01);
+ }
+ else if(buffer[0] == 'd')
+ {
+ int kd1=int(buffer[1]-48)*100+int(buffer[2]-48)*10+int(buffer[3]-48)*1;
+ if(kd1<1000)
+ {
+ kd=kd1;
+ }
+ wait(0.01);;
+ }
+ else if(buffer[0] == 'i')
+ {
+ int ki1=int(buffer[1]-48)*100+int(buffer[2]-48)*10+int(buffer[3]-48)*1;
+ if(ki1<1000)
+ {
+ ki=ki1;
+ }
+ wait(0.01);
+ }
+ cleanBuffer(buffer,6);
+ enviarvalores();
+ }
+
+ }
+
+
+}
\ No newline at end of file