Uso del encoder para un PID

Dependencies:   QEI TextLCD mbed

Files at this revision

API Documentation at this revision

Comitter:
ikortegag
Date:
Wed Dec 11 21:30:11 2013 +0000
Commit message:
Implementaci?n de un encoder para un PID

Changed in this revision

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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Wed Dec 11 21:30:11 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TextLCD.lib	Wed Dec 11 21:30:11 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ikortegag/code/TextLCD/#0c9d76d82120
--- /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);
+    }
+
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Dec 11 21:30:11 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file