Katherine Ortega
Uso del encoder para un PID
Dependencies: QEI TextLCD mbed
Diff: main.cpp
- Revision:
- 0:9575323f2bb3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Wed Dec 11 21:30:11 2013 +0000
@@ -0,0 +1,268 @@
+#include "mbed.h"
+#include "TextLCD.h"
+#include "QEI.h"
+
+
+//Use X4 encoding.
+//QEI wheel(p29, p30, NC, 624, QEI::X4_ENCODING);
+//Use X2 encoding by default.
+
+AnalogIn Vin(PTC2);
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
+QEI wheel (PTA1, PTA2, NC, 624);
+
+AnalogIn y(PTB0);
+AnalogOut u(PTE30);
+
+DigitalOut l1(LED1);
+DigitalOut l2(LED2);
+DigitalOut l3(LED3);
+DigitalIn b1(PTA1);
+DigitalIn b2(PTA2);
+DigitalIn b3(PTC16);
+DigitalIn b4(PTC17);
+
+int spnum=0,kpnum=0,kinum=0,kdnum=0,k=1;
+int bandera;
+Timer t;
+float tiempo;
+
+//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 err, med, yr, pid, ap, ai, ad, err_v, cycle;
+float pidn;
+int pos=1;
+//int spnum=500, kinum=2, kpnum=1, kdnum=0, pos=1;
+//int incremento=1,flagt=0;
+
+int main()
+{
+ 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)
+ {
+ wait(0.1);
+ lcd.printf("Pulses is: %i\n", wheel.getPulses());
+
+ while(k==1)
+ {
+ if(b1 & spnum<=1000)
+ {
+ l1 = !l1;
+ spnum++;
+ lcd.locate(3,0);
+ lcd.printf("%d",spnum);
+ }
+
+ if(b2 & spnum>=1)
+ {
+ l2 = !l2;
+ wait(0.50);
+ spnum--;
+ lcd.locate(3,0);
+ lcd.printf("%d",spnum);
+ }
+
+ if (b3)
+ {
+ k+=1;
+ wait(0.25);
+ lcd.locate(12,0);
+ lcd.printf("%d",kpnum);
+ }
+
+ }
+
+ while(k==2)
+ {
+ if(b1 & kpnum<=1000)
+ {
+ l1 = !l1;
+ wait(0.50);
+ kpnum++;
+ lcd.locate(12,0);
+ lcd.printf("%d",kpnum);
+ }
+ if(b2 & kpnum>=1)
+ {
+ l2 = !l2;
+ wait(0.50);
+ kpnum--;
+ lcd.locate(12,0);
+ lcd.printf("%d",kpnum);
+
+ }
+
+ if (b3)
+ {
+ k+=1;
+ wait(0.25);
+ lcd.locate(3,1);
+ lcd.printf("%d",kinum);
+ }
+ }
+
+ while(k==3)
+ {
+ if(b1 & kinum<=1000)
+ {
+ l1 = !l1;
+ wait(0.50);
+ kinum++;
+ lcd.locate(3,1);
+ lcd.printf("%d",kinum);
+ }
+ if(b2 & kinum>=1)
+ {
+ l2 = !l2;
+ wait(0.50);
+ kinum--;
+ lcd.locate(3,1);
+ lcd.printf("%d",kinum);
+ }
+ if (b3)
+ {
+ k+=1;
+ wait(0.25);
+ lcd.locate(12,1);
+ lcd.printf("%d",kdnum);
+ }
+ }
+
+ while(k==4)
+ {
+ if(b1 & kdnum<=1000)
+ {
+ l1 = !l1;
+ wait(0.50);
+ kdnum++;
+ lcd.locate(12,1);
+ lcd.printf("%d",kdnum);
+ }
+ if(b2 & kdnum>=1)
+ {
+ l2 = !l2;
+ wait(0.50);
+ kdnum--;
+ lcd.locate(12,1);
+ lcd.printf("%d",kdnum);
+ }
+
+ if (b3)
+ {
+ k=1;
+ wait(0.25);
+ lcd.locate(3,0);
+ lcd.printf("%d",spnum);
+ }
+
+ }
+
+
+ if (b4)
+ {
+ break; //sale del bucle si pisan suiche4
+ }
+ continue;
+ }
+
+
+ //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=%d",err);
+ lcd.locate(8,0);
+ lcd.printf("Me=%d",med);
+ lcd.locate(0,1);
+ lcd.printf("Sp=%d",spnum);
+ lcd.locate(8,1);
+ lcd.printf("Co=%d",pid);
+ wait(2);
+
+ // CICLO PRINCIPAL CONTROLADOR PID
+ bandera=0;
+ while(1)
+ {
+ med=999*y.read(); //leer puerto analogo y asignar a med
+ err = (spnum-med); //se calcula el error
+
+ ap = kpnum*err; //se calcula la accion proporcinal
+
+ // se verifica que la accion integral no sea muy grande
+ if(ai<100)
+ {
+ ai =(kinum*err)+ai; //calculo de la integral del error
+ }
+
+ ad = kdnum*(err-err_v); //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 actualizan las variables *******************************************
+ err_v = err;
+
+ //se muestran las variables******************************************
+ if(bandera==0)
+ {
+ t.start();
+ bandera=1;
+ }
+ if(t>=0.3)
+ {
+ lcd.locate(3,0);lcd.printf(" ");
+ lcd.locate(3,0);lcd.printf("%d",err);
+ lcd.locate(11,0);lcd.printf(" ");
+ lcd.locate(11,0);lcd.printf("%d",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("%d",pid);
+ bandera=0;
+ t.reset();
+ }
+
+ //Normalizacion de la salida
+ pidn=pid/999;
+ // se envia el valor pid a puerto analogico de salida (D/A) **************
+ u.write(pidn);
+ // se repite el ciclo
+ wait(0.005);
+ }
+
+}