a

Dependencies:   QEI TextLCD mbed

Fork of PID_ENCODER_OK by Gustavo Ramirez

Revision:
0:4e0dfcf0e7ce
Child:
1:058b8f5c135d
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 22 23:11:38 2016 +0000
@@ -0,0 +1,326 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "TextLCD.h"
+
+TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
+QEI encoder (PTA13, PTD5, NC, 624);
+
+AnalogIn y(PTB0);//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
+
+
+//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, cambio=0, diferencia=0;
+float pidn;
+int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1;
+int flagt=0;
+Timer t;
+
+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);
+
+        diferencia=encoder.getPulses()-cambio;
+        cambio=encoder.getPulses();
+
+        if (diferencia==0)
+        {
+            //nada
+        }
+        else if(diferencia>0)
+        {
+            if(pos==1)
+            {
+                if(spnum+diferencia>=9999)
+                {
+                    spnum=9999;
+                    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>=9999)
+                {
+                    kpnum=9999;
+                    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>=9999)
+                {
+                    kinum=9999;
+                    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>=9999)
+                {
+                    kdnum=9999;
+                    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=%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
+    flagt=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(flagt==0)
+        {
+            t.start();
+            flagt=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);
+            flagt=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);
+    }
+}