Controlador PID

Dependencies:   Debounced Pulse1 QEI RTC-DS1307 TextLCD_1 mbed

Fork of grafica_PID_android by Nicolás Villegas Echavarría

/media/uploads/seccastanova/whatsapp_image_2018-06-05_at_10.25.03_am.jpeg

Revision:
2:00db719ebd81
Parent:
1:058b8f5c135d
Child:
3:2e823b16a4a1
diff -r 058b8f5c135d -r 00db719ebd81 main.cpp
--- a/main.cpp	Sun Apr 24 18:32:52 2016 +0000
+++ b/main.cpp	Wed May 10 22:36:03 2017 +0000
@@ -1,306 +1,251 @@
+/*
+ * Código por: Laura Álvila
+ *             Daniela López
+ *             Nicolás Villegas            
+ */
+ 
 #include "mbed.h"
 #include "QEI.h"
 #include "TextLCD.h"
 
+Serial GSM(PTE0,PTE1);  // Módulo Bluetooh
 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
-//si se ignora esto se arruina la FRDMKL25Z
+QEI encoder (PTA13, PTD5, NC, 1000);
+
+
+AnalogIn y(PTC2);   // Entrada análoga: Salida del sistema
+AnalogOut u(PTE30); // Salida análoga: Señal de control
+
 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
-
+DigitalIn button3(PTC16); // Botón del encoder
+DigitalIn button4(PTC17); // Pulsador
 
-//codigos movimiento del curzor
-
-//int C1=0x0E; // solo muestra el curzor
+// Códigos movimiento del cursor:
+int C1=0x0F;
 int C2=0x18; // desplaza izquierda
 int C3=0x1A; // desplaza derecha
 int C4=0x0C; // quito cursor bajo
 
-int C1=0x0F;
-int cambio=0, diferencia=0;
-float pid,o,ai,ad,ap,med,err;
+int cambio = 0, diferencia = 0;
+float pid, o, ai, ad, ap, med, err;
 float err_v;
-int spnum=0,kinum=0,kpnum=0,kdnum=0,pos=1;
+int spnum = 0, kinum = 0, kpnum = 0 ,kdnum = 0, pos = 1;
+int j,k; 
 
-int main()
-{
+int main() {
+    // Se asigna baudrate y se configura el puerto serie de la USART
+    GSM.baud(9600);   
+    GSM.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
+    wait(3);
+    
+    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.printf("Kp=%d", kpnum);
     lcd.locate(0,1);
-    lcd.printf("Ki=%d",kinum);
+    lcd.printf("Ki=%d", kinum);
     lcd.locate(8,1);
-    lcd.printf("Kd=%d",kdnum);
+    lcd.printf("Kd=%d", kdnum);
     lcd.locate(0,0);
-    lcd.printf("Sp=%d",spnum);
+    lcd.printf("Sp=%d", spnum);
 
-    while(1)
-    {
-        //lcd.locate(8,0);
-        //lcd.printf("Kp=%d",encoder.getPulses());
-        //wait(.5);
+    while(true) {
 
-        diferencia=encoder.getPulses()-cambio;
+        diferencia=encoder.getPulses() - cambio;
         cambio=encoder.getPulses();
-
-        if (diferencia==0)
-        {
-            //nada
-        }
-        else if(diferencia>0)
-        {
-            if(pos==1)
-            {
-                if(spnum+diferencia>=999)
-                {
-                    spnum=999;
+        
+        if (diferencia != 0) {
+            switch (pos) {
+                case 1:
+                    spnum += diferencia;
+                    
+                    if (spnum >= 999)
+                        spnum = 999;
+                    else if (spnum < 0)
+                        spnum = 0;
+                        
                     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;
+                    break;
+                case 2:
+                    kpnum += diferencia;
+                    
+                    if (kpnum >= 999)
+                        kpnum = 999;
+                    else if (kpnum < 0)
+                        kpnum = 0;
                     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;
+                    break;
+                case 3:
+                    kinum += diferencia;
+                    
+                    if (kinum >= 999)
+                        kinum = 999;
+                    else if (kinum < 0)
+                        kinum = 0;
+
                     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;
+                    break;
+                case 4:
+                    kdnum += diferencia;
+                    
+                    if (kdnum >= 999)
+                        kdnum = 999;
+                    else if (kdnum < 0)
+                        kdnum = 0;
+
                     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);
-                }
-            }
-        }
+                    break;
+                default:
+                    break;
+            } // Fin switch
+        } // Fin if
+
+        if (!button3) { // Cambia la posición de ingreso de parámetros
         
-        else if(diferencia<0)
-        {
-            if(pos==1)
-            {
-                if(spnum+diferencia<0)
-                {
-                    //No ocurre nada
-                }
-                else
-                {
-                    spnum+=diferencia;
-                    lcd.locate(3,0);
-                    lcd.printf("    ");
+            led3 = !led3;
+            
+            switch(pos++) {
+                case 4:
+                    pos = 1;
                     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("    ");
+                    break;
+                case 1:
                     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("    ");
+                    break;
+                case 2:
                     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("    ");
+                    break;
+                case 3:
                     lcd.locate(11,1);
                     lcd.printf("%d", kdnum);
-                }
-            }
-        }
+                    break;
+                default:
+                    break;
+            } // Fin switch
+            wait(0.25);
+        } // Fin if
+              
 
-        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)
-        {
+        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
+// Transición
+    lcd.writeCommand(C4); // Escribimos un comando segun el manual del módulo LCD para quitar cursor bajo
+    lcd.cls(); // Borra la pantalla
     lcd.printf("   GUARDADOS!");
     wait(1);
     lcd.cls();
-    lcd.printf(" INICIA EL PID");
+    lcd.printf("  *** PID ***");
     wait(1);
-// se imprimen los parches del control  *****************************************
+
     lcd.cls();
-    lcd.printf("Er=%3.0f",err);
+    lcd.printf("e=%3.0f",err);
     lcd.locate(8,0);
-    lcd.printf("Me=%3.0f",med);
+    lcd.printf("y=%3.0f",med);
     lcd.locate(0,1);
-    lcd.printf("Sp=%3.0f",spnum);
+    lcd.printf("r=%3.0f",spnum);
     lcd.locate(8,1);
-    lcd.printf("Co=%3.0f",pid);
+    lcd.printf("u=%3.0f",pid);
     wait(1);
 
+int referencia;
+int salida;
+
 // 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;
+loop:   med = y.read()*999;
+        err = spnum - med;      // Se calcula el error
+        ap = kpnum*err*0.01f;   // Se calcula la acción proporcional
+        ai += kinum*err*0.01f;  // Cálculo de la integral del error
+        ad = kdnum*(err - err_v)*0.01f; // Cálculo de la acción derivativa
+        pid = ap + ai + ad;
+        
+        referencia = (spnum / 999) * 255;
+        salida = med / 999 * 743 + 256;
+        
+        if (referencia < 256) {        // Debo generar dos casos a APP inventor solo me recibe hex asi: 0xhhhh (4 cifras)    
+            GSM.putc(0);         // si el numero es hasta 255 se le ponen dos ceros adelante a la secuencia de bits
+            GSM.putc(referencia);       // luego la cifra menos significativa
         }
-
-        // se verifica que pid sea menor o igual la valor maximo *****************
-        if (pid > 999)
-        {
-            pid=999;
+                /*  
+        
+        if (salida > 255){         // pero si es mayor a 255 las cifras deben ser convertidas a un hex de dos bytes de la siguiente forma   
+           j = salida/256;         // calculo la cifra mas significativa
+           k = salida - j*256;     // calculo la cifra menos significativa
+           GSM.putc(j);         // las envio a la usart para que se las ponga al modulo bluetooth y la lleve al android
+           GSM.putc(k);         // mas significativa primero, menos despues si no no funciona!!! y con la orden PUTC solo asi le envia binarios
+       }
+       
+ 
+        GSM.putc('r');          // Se envía el set-point al módulo Bluetooth.
+        if (spnum < 256) {    
+           GSM.putc(0);
+           GSM.putc(spnum);
         }
+        if (spnum > 255){  
+           j = spnum/256;
+           k = spnum - j*256;
+           GSM.putc(j);
+           GSM.putc(k);
+       }
+       */
+       
+        // Se verifica que pid sea positivo:
+        if (pid <= 0)
+            pid = 0;
+        else 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);
+        // Se muestran las variables en LCD:
+        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);
            
-            
-        
-
-        //Normalizacion de la salida
-        // se actualizan las variables *******************************************
+        // Normalización de la salida y actualización de 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
+        // Se repite el ciclo:
         wait_ms(300);
-        goto lop1;
+        goto loop;
 }