PID que funciona con encoder y tiene autotunning.

Dependencies:   QEI TextLCD mbed

Fork of TAREA_4_PROCESADORES by john lopez

Files at this revision

API Documentation at this revision

Comitter:
PROCESADORES_2017_2
Date:
Tue Nov 28 19:47:14 2017 +0000
Parent:
1:9ca362d07dd0
Commit message:
PID que funciona con encoder y tiene autotunning.

Changed in this revision

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
diff -r 9ca362d07dd0 -r f854af43e0c2 main.cpp
--- a/main.cpp	Tue Apr 08 19:02:44 2014 +0000
+++ b/main.cpp	Tue Nov 28 19:47:14 2017 +0000
@@ -5,31 +5,49 @@
  
 AnalogIn Vin(PTC2);
 AnalogOut Vout(PTE30);
-PwmOut sound(PTA12);
+
 
-TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); //Puertos LCD rs, e, d4, d5, d6, d7
+TextLCD lcd(PTB8, PTB9,PTB10,PTB11,PTE2, PTE3); //Puertos LCD rs, e, d4, d5, d6, d7
 QEI Encoder (PTA1, PTA2, NC, 624);               //Puertos de la tarjeta asignados para el Encoder
  
 DigitalOut led1(LED1);      //led de cambio de posición
 DigitalOut led2(LED2);      //led incremento de parámetros
 DigitalOut led3(LED3);      //led decremento de parámetros
-DebouncedIn bot1(PTC12);    //cambiar la posición (CONECTO ESTA ENTRADA A LA ULTIMA SALIDA DEL ENCONDER, ASI SE UTILIZARA EL BOTON DEL ENCODER)
-DebouncedIn bot4(PTC17);    //salida de bucle
+
+Timer t;
+int B,t2,tim;
+DebouncedIn bot1(PTC17);    //cambiar la posición 
+    //salida de bucle
   
    //Códigos LCD
 int C1=0x0E;                // Muestra el cursor
 // 0x18;                // desplazamiento izquierda
 // 0x1A;                // desplazamiento derecha
 int C4=0x0C;
+//Variables autotuning----------------------------------------------------------
+int TC;
+float kc;
+float ko;
+float v2;
+int AT=0;
+int i=1;
+int j=1;
+int k=1;
+int q;
+int q2;
+char D[30];
+int v6;
 
-
+//------------------------------------------------------------------------------
 int a, kp, ki, kd, sp, ciclo, valor, v; // indice de la variable
 float  med, sp0, pid, ap, err, ai, ad,err_v;
-
-
- 
+Timer o;
+ int o2;
+int p=1;
 int main() {
-
+float Kp,Kd,Ki;
+    t.reset();
+    
     led1=led2=led3=1;
     lcd.printf("Control PID");
     wait(1.5);
@@ -49,21 +67,217 @@
    
      //Inicio del ciclo
      
-     while(1) {
-           if (bot1.falling()) {            //----------------- Aumenta de posición el cursor a la primera línea de menu
+     while(1) {  if (bot1==0 && kp==0 && ki==0 && kd==0 && sp==0) 
+                 {AT=1;      }
+                 
+                 if (AT==1){ 
+                                                if(p==1){
+                                                lcd.cls();       //limpio la pantalla  
+                                                lcd.locate(5,1);
+                                                lcd.printf("AUTOTUNING");
+                                                wait(0.5);
+                                                lcd.cls();
+                                                p=2;
+                                                Kp=0.5;
+                                                sp=3;
+                                                
+                                                }
+                                            if (p==2){
+                                              for (Kp=0.5;Kp<2;Kp=Kp+0.1){
+                                                
+                                                if (j==1 && k==1 && v6==0){
+                                                for (i=1;i<20;i++){
+                                                            pid=999;
+                                                            Vout=(pid/999); 
+                                                            v2=(Vin.read())*3300;//la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V)
+                                                            float kp0;
+                                                            sp0 = sp;
+                                                            err = (sp0*1000-v2); //Estoy ingresando un valor en V, pero opero internamente en mV
+                                                            kp0 = Kp;
+                                                            ap = kp0*err;
+                                                            pid = ap;
+                                                            
+                                                            if (pid>999){pid=999;}
+                                                            if (pid<0){pid=0;}
+                                                
+                                                            Vout=(pid/999); 
+                                                          
+                                                            lcd.locate(5,1);
+                                                            lcd.printf("Leyendo..");   
+                                                            D[i]=v2/1000;
+                                                            
+                                                            led1=0;
+                                                            wait(1);
+                                                            led1=1;
+                                                            }
+                                                            }
+                                                                  lcd.cls();                       
+                                                if (i==20 && k==1 && v6==0){
+                                                
+                                                for (j=1;j<20;j=j+2)    {
+                                                 //   lcd.locate(0,0);
+                                                   // lcd.printf("j %d",j); 
+                                                                                      
+                                                            if (D[j]==D[j+2]){ 
+                                                            led2=0;
+                                                            q=q++;
+                                                            //lcd.locate(8,0);
+                                                            //lcd.printf("Q %d",q);
+                                                            lcd.locate(5,1);
+                                                            lcd.printf("Operando..");
+                                                            wait(1);
+                                                            
+                                                                                }
+                                                                        }
+                                                                        
+                                                                    }
+                                                lcd.cls();
+                                                
+                                                if(j==21 && i==20 && k!=21 && v6==0){
+                                                    for(k=1;k<20;k=k+2) {
+                                                // lcd.locate(0,0);
+                                                  //  lcd.printf("k %d",k);     
+                                                    if (D[k+1]==D[k+3]){ 
+                                                    led3=0;
+                                                    led2=1;
+                                                    q2=q2++;
+                                                    //lcd.locate(0,1);
+                                                    //lcd.printf("Q2 %d",q2);
+                                                      lcd.locate(5,1);
+                                                    lcd.printf("Analisis...");
+                                                    wait(1);}
+                                                    
+                                                                    }
+                                                                                                
+                                                       }
+                                                                                             
+                                                        lcd.cls();
+                                                
+                                                 
+                                                if (i==20 && j==21 && k==21 && q>8 && q2>8){
+                                                    led2=0;
+                                                    kc=Kp;
+                                                    p=3;
+                                                    v6=1;
+                                                    lcd.locate(5,1);
+                                                    lcd.printf("Terminado");
+                                                    wait(1);
+                                                    lcd.cls();
+                                                    lcd.printf("KC: %4.2f",Kp);
+                                                    wait(1);
+                                                    
+                                                    break;
+                                                                }
+                                                    if(i==20 && j>20 && k==21 && q<=8 && q2<=8){
+                                                                        lcd.locate(5,1);
+                                                                        lcd.printf("Terminado");
+                                                                        wait(1);
+                                                                        lcd.cls();
+                                                                        lcd.locate(5,1);
+                                                                        lcd.printf("NO ES LA KC");
+                                                                        wait(1);
+                                                                        lcd.cls();
+                                                                       
+                                                                        i=1;
+                                                                        j=1;
+                                                                        k=1;
+                                                                        q=0;
+                                                                        q2=0;
+                                                                        }
+                                                                            }
+                                                                                               
+                                                }                                                               
+                                             
+                                             if (v6==1){
+                                                 lcd.cls();
+                                                 lcd.locate(5,1);
+                                                 lcd.printf("Calculando..");
+                                                 TC=1;
+                                                 Kp=0.59*kc;
+                                                 Ki=1.18*kc*(1/TC);
+                                                 Kd=0.074*kc*TC;
+                                                    
+                                                 lcd.cls();
+                                                 lcd.printf("INICIA EL PID");
+                                                 wait(2);
+                                                               // se imprimen los parches del control  *****************************************
+                                                
+                                                   lcd.locate(0,0);
+                                                   lcd.printf("E:%d");
+                                                   lcd.locate(8,0);
+                                                   lcd.printf("Y:%d");
+                                                   lcd.locate(0,1);
+                                                   lcd.printf("Sp:%d");
+                                                lcd.locate(8,1);
+                                                lcd.printf("Co:%0.1f");
+                                              // CICLO PRINCIPAL CONTROLADOR PID
+                                                                                 
+                                                                                 while(1) {
+                                                                                   
+                                                                             wait(0.001);
+                                             //                                                                                                        
+                                                                             v2 = (Vin.read()*3300);//la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V)
+                                                                             sp0 = sp;
+                                                                             err = (sp0*1000-v2); //Estoy ingresando un valor en V, pero opero internamente en mV
+                                                                             float kp0;
+                                                                             kp0 = Kp;
+                                                                             ap = kp0*err;
+                                                                             float ki0;
+                                                                             ki0 = Ki;        
+                                                                             ai =  (ki0*err)+ai;     //calculo de la integral del error
+                                                                             float kd0;
+                                                                             kd0 =Kd;
+                                                                             ad =  kd0*(err-err_v);  //calculo de la accion derivativa
+                                                                             pid = (ap+ai+ad);
+                                                                            // se verifica que pid sea menor o igual la valor maximo *****************
+                                                                             if (pid > 999){ pid=999;} 
+                                                                         // se verifica que pid sea positivo **************************************
+                                                                                  if (pid < 0){ pid=0;} 
+                                                                                  // se verifica que la accion integral no sea muy grande
+                                                                                   if (ai > 999){ai=1000;} 
+                                                                                  //  
+                                                                                           
+                                                                                  Vout=(pid/999);                
+                                                                                                        //Mostrar resultados PID
+                                                                                  if(ciclo>700)  {         
+                                                                                  lcd.locate(2,0);
+                                                                                  lcd.printf("      "); 
+                                                                                  lcd.locate(0,0);
+                                                                                  lcd.printf("E:%4.2f",err/1000);
+                                                                                  lcd.locate(10,0);
+                                                                                  lcd.printf("      ");
+                                                                                lcd.locate(8,0);
+                                                                                  lcd.printf("Y:%4.2f",v2/1000);
+                                                                                  lcd.locate(2,1);
+                                                                                           lcd.printf("      ");
+                                                                                        lcd.locate(0,1);
+                                                                                       lcd.printf("Sp:%4.2f",sp0);
+                                                                                        lcd.locate(10,1);
+                                                                                   lcd.printf("      ");
+                                                                                   lcd.locate(11,1);
+                                                                                    lcd.printf(":%0.1f",pid*3.3/999);
+                                                                                    ciclo=0;
+                                                                                              }
+                                                                                            else 
+                                                                                               ciclo++;     
+                                                                                               err_v = err;  //guarda el error 
+                                                                                                }      //  Envía parámetro pid al puerto analogico de salida (D/A) y se repite el cicl                                                                                                            
+                   }
+                   } 
+                    else{                                         
+                    if (AT==0){
+                
+                            if (bot1.falling()&&B==0 && kp==0 && ki==0 && kd==0) {            //----------------- Aumenta de posición el cursor a la primera línea de menu
+                      
                
-            sound=1.0f;
-            wait(0.1);
-            sound=0;
-               
-               led1=0;
-               wait(.15); 
-               led1=1;
-               ++a;
-               }
+                led1=0;
+                wait(.15); 
+                led1=1;
+                ++a;
+                }
          
                         valor = Encoder.getPulses();    //------------- Asigna el valor de los pulsos del encoder a una variable llamada "valor"                                                
-                       
+                       if(B==0 && t2==0){
                         switch(a) {
             
                         case 0:
@@ -73,6 +287,9 @@
                         if (sp<0){
                             sp=0;                       //------------- No se admite valores negativos
                             }
+                        if (sp>3){                      //Limito el valor del Set Point;
+                            sp=3;
+                            }
                         
                         lcd.locate(2,0);                //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD
                         lcd.printf("=    ",sp);
@@ -80,12 +297,8 @@
                         lcd.printf("%i",sp);
                         wait(0.15);
                         
-                            if (bot1.falling()) {       //--------------Aumenta de posición el cursor a la segunda línea de menu
-                            
-                            sound=1.0f;
-                            wait(0.1);
-                            sound=0;
-                            
+                            if (bot1.falling() && sp>0 && a==0 &&B==0) {       //--------------Aumenta de posición el cursor a la segunda línea de menu
+                         
                             a=1;
                             led1=0;
                             wait(.15); 
@@ -101,6 +314,7 @@
                                 if (kp<0){
                                 kp=0;                   //------------- No se admite valores negativos
                                 }
+                                if (kp>999){ kp=999;}  //Limito el valor de KP
                         
                                 lcd.locate(10,0);       //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD
                                 lcd.printf("=    ",kp);
@@ -108,11 +322,9 @@
                                 lcd.printf("%i",kp);                        
                                 wait(0.15);
                                    
-                                    if (bot1.falling()) { //--------------Aumenta de posición el cursor a la tercera línea de menu    
+                                    if (bot1.falling()&& kp>0 && a==1 && B==0) { //--------------Aumenta de posición el cursor a la tercera línea de menu    
                                     
-                                    sound=1.0f;
-                                    wait(0.1);
-                                    sound=0;
+                                    
                                     
                                     a=2;
                                     led1=0;
@@ -129,19 +341,15 @@
                                         if (ki<0){
                                         ki=0;             //------------- No se admite valores negativos
                                         }
-                                        
+                                        if (ki>999){ ki=999;}  //Limito el valor de KI
                                         lcd.locate(2,1);  //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD
                                         lcd.printf("=    ",ki);
                                         lcd.locate(3,1);
                                         lcd.printf("%i",ki);                                            
                                         wait(0.15);
                                         
-                                           if (bot1.falling()) { //--------------Aumenta de posición el cursor a la cuarta línea de menu         
-                                            
-                                            sound=1.0f;
-                                            wait(0.1);
-                                            sound=0;
-                                            
+                                           if (bot1.falling()&&ki>0 && a==2 && B==0) { //--------------Aumenta de posición el cursor a la cuarta línea de menu         
+                                                                                                                              
                                             a=3;
                                             led1=0;
                                             wait(.15); 
@@ -156,17 +364,16 @@
                                                 if (kd<0){
                                                 kd=0;            //------------- No se admite valores negativos
                                                 }
+                                                if (kd>999){ kd=999;}   //Limito el valor de KD
                                                 lcd.locate(10,1);//------------- Ubica e imprime el parámetro "kd" en la pantalla LCD
                                                 lcd.printf("=    ",kd);
                                                 lcd.locate(11,1);
                                                 lcd.printf("%i",kd);
                                                 wait(0.15);
                                                 
-                                                    if (bot1.falling()) {  //--------------Aumenta de posición el cursor a la cuarta línea de menu    
-                                                    
-                                                    sound=1.0f;
-                                                    wait(0.1);
-                                                    sound=0;
+                                                
+                                                    if (bot1.falling()&&kd>0 &&a==3 && B==0) {  //--------------Aumenta de posición el cursor a la cuarta línea de menu    
+                                                                                                       
                                                     
                                                     a=0;
                                                     led1=0;
@@ -174,99 +381,113 @@
                                                     led1=1;
                                                     Encoder.reset();       //------------- Resetea el valor del encoder
                                                     }  
-                                                break;
-                                                
-                                                  }
-                                                                        
-                                                   if (bot4.falling()){
-                                                   led1=led2=led3=0;       //------------- Flash para salir del bucle
-                                                   wait(0.25);
-                                                   led1=led2=led3=1;
-                                                   break;                  //------------- sale del bucle de la pantalla
-                                             }
-                                         }
-                 
-//---------------------------------------------------------------------------------------------------------------------------------------------------
-           lcd.writeCommand(C4);                                   //Quita el cursor bajo de la pantalla LCD
-           lcd.cls();                                              //borra la pantalla
-           lcd.printf("GUARDADOS!"); 
-           wait(2);
-           lcd.cls();
-           lcd.printf("INICIA EL PID");
-           wait(2);
-                                                                    // 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",sp0);
-           lcd.locate(8,1);
-           lcd.printf("Pid%d",pid);
-           
-           
-           while(1) {
-           
-           wait(0.001);
-                                                                    //leer puerto analogo y asignar a med
-           
-           
-           med = (Vin*3.27);
-           sp0 = sp*0.0327;
-           err = (sp0-med); 
-           float kp0;
-           kp0 = kp*0.001;
-           ap = kp0*err;
-           float ki0;
-           ki0 = ki*0.001;      
-           ai =  (ki0*err)+ai;     //calculo de la integral del error
-           float kd0;
-           kd0 = kd*0.0001;
-           ad =  kd0*(err-err_v);  //calculo de la accion derivativa  
-           pid = (ap+ai+ad);
-           
-           
-              // se verifica que pid sea menor o igual la valor maximo *****************
-           if (pid > 0.999){
-           pid=1;
-           } 
-           
-           // se verifica que pid sea positivo **************************************
-           if (pid < 0){
-           pid=0;
-           } 
-                 
-           // se verifica que la accion integral no sea muy grande
-           if (ai > 999){
-           ai=1000;
-           } 
-                     
-           Vout=pid;                
+                                                   break;                                              
+                                                }
+                                }            
+                                //PARA GUARDAR LOS PARAMETROS SE PRESIONA EL PULSADOR DEEL ENCODER 5 SEG
+                    if (bot1==0 && kp>0 && ki>0 && kd>0)
+                                 { 
+                                   t.start(); //inicio el timer
+                                   t2=t.read();//leo el valor del timer y lo almaceno en una variable entera "t2"
+                                   B=1;
+                                   
+                                                                                                                                                                                                                
+                                                   if(t2==5){
+                                                              t.stop(); //paro el timer
+                                                              t.reset();//lo reseteo
+                                                              led3=0;   //led indicador-- enciendo led azul
+                                                              B=0;
+                                                            lcd.cls();
+                                                           }
+                                                     
+                                                   if (t2==5 && B==0)
+                                                     {      
+                                                      lcd.cls();                                              //borra la pantalla
+                                                      lcd.printf("GUARDADOS!"); 
+                                                      wait(2);
+                                                      lcd.cls();
+                                                      lcd.printf("INICIA EL PID");
+                                                      wait(2);
+                                                       lcd.cls();
+                                                    //Se imprimen los parches de control 
+                                                       lcd.printf("E=%d");   //Error
+                                                       lcd.locate(8,0);  
+                                                       lcd.printf("Y=%d");   //Salida
+                                                       lcd.locate(0,1);
+                                                       lcd.printf("Sp=%d");   //Set point
+                                                       lcd.locate(8,1);
+                                                       lcd.printf("Co=%3.0f"); //Esfuerzo de control
+                                                       wait(1);
+                                                                      
+                                                                   
+                                                  //CICLO PID        
+                                                       
+                                                 while (true){
+                                                       wait(0.001);
+                                                       med = (Vin.read()*3300); //la medicion es convertida a un valor en mV,siendo el máximo valor esperado 3300mV (3.3V)
+                                                       sp0 = sp;
+                                                       err = (sp0*1000-med); //Estoy ingresando un valor en V, pero opero internamente en mV
+                                                       float kp0;
+                                                       kp0 = kp;
+                                                       ap = kp0*err;
+                                                       float ki0;
+                                                       ki0 = ki;      
+                                                       ai =  (ki0*err)+ai;     //calculo de la integral del error
+                                                       float kd0;
+                                                       kd0 = kd;
+                                                       ad =  kd0*(err_v-err);  //calculo de la accion derivativa  
+                                                       pid = (ap+ai+ad);
+                                                    
+                                                                                                                                                                  
+                                                       // se verifica que pid sea positivo
+                                                       if (pid < 0){
+                                                       pid=0;}
+                                                      // Se limita el valor del PID
+                                                           if (pid > 999)
+                                                    {   pid=999;    }
+                                             
+                                                       // se verifica que la accion integral no sea muy grande
+                                                       if (ai > 999){
+                                                       ai=1000;
+                                                       } 
+                                                   Vout=(pid/999);              
+                                                                     
                                 //Mostrar resultados PID
-          if(ciclo>700)  {         
-           lcd.locate(2,0);
-           lcd.printf("      "); 
-           lcd.locate(0,0);
-           lcd.printf("Er%2.1f",err);
-           lcd.locate(10,0);
-           lcd.printf("      ");
-           lcd.locate(8,0);
-           lcd.printf("Me%4.2f",med);
-           lcd.locate(2,1);
-           lcd.printf("      ");
-           lcd.locate(0,1);
-           lcd.printf("Sp%4.2f",sp0);
-           lcd.locate(10,1);
-           lcd.printf("      ");
-           lcd.locate(8,1);
-           lcd.printf("Pid%4.3f",pid);
-           ciclo=0;
-           }
-           else 
-           ciclo++;     
-           err_v = err; //guarda el error  
-          }      //  Envía parámetro pid al puerto analogico de salida (D/A) y se repite el ciclo
-      }
+                                          if(ciclo>700)  {         
+                                           lcd.locate(2,0);
+                                           lcd.printf("      "); 
+                                           lcd.locate(0,0);
+                                           lcd.printf("E=%4.2f",err/1000);
+                                           lcd.locate(10,0);
+                                           lcd.printf("      ");
+                                           lcd.locate(8,0);
+                                           lcd.printf("Y=%4.2f",med/1000);
+                                           lcd.locate(2,1);
+                                           lcd.printf("      ");
+                                           lcd.locate(0,1);
+                                           lcd.printf("Sp=%4.2f",sp0);
+                                           lcd.locate(11,1);
+                                            lcd.printf("   ");
+                                            lcd.locate(11,1);
+                                            lcd.printf("%0.1f ",pid*3.3/999);
+                                           
+                                           ciclo=0;
+                                           }
+                                           else 
+                                           ciclo++;     
+                                           err_v = err; //guarda el error  
+                                           
+                                            //  se repite el ciclo
+        
+        }
+        }
+                                                  }     
+                                          }    
+                        }
+                  }
+         }
+                                    
+                                    
+                                  
+       
        
\ No newline at end of file
diff -r 9ca362d07dd0 -r f854af43e0c2 mbed.bld
--- a/mbed.bld	Tue Apr 08 19:02:44 2014 +0000
+++ b/mbed.bld	Tue Nov 28 19:47:14 2017 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/a9913a65894f
\ No newline at end of file
+http://mbed.org/users/mbed_official/code/mbed/builds/e7ca05fa8600
\ No newline at end of file