PID que funciona con encoder y tiene autotunning.

Dependencies:   QEI TextLCD mbed

Fork of TAREA_4_PROCESADORES by john lopez

main.cpp

Committer:
lopjohn_26
Date:
2014-04-08
Revision:
1:9ca362d07dd0
Parent:
0:dd68101b1b99
Child:
2:f854af43e0c2

File content as of revision 1:9ca362d07dd0:

#include "mbed.h"
#include "TextLCD.h"
#include "DebouncedIn.h"
#include "QEI.h"
 
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
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
  
   //Códigos LCD
int C1=0x0E;                // Muestra el cursor
// 0x18;                // desplazamiento izquierda
// 0x1A;                // desplazamiento derecha
int C4=0x0C;


int a, kp, ki, kd, sp, ciclo, valor, v; // indice de la variable
float  med, sp0, pid, ap, err, ai, ad,err_v;


 
int main() {

    led1=led2=led3=1;
    lcd.printf("Control PID");
    wait(1.5);
    lcd.cls();
    
    lcd.writeCommand(C1);   //comando para mostrar el cursor en el LCD
                       
    lcd.locate(0,0);        // Ubica e imprime nombre de los parámetros en del PID en la pantalla LCD
    lcd.printf("Sp=");
    lcd.locate(8,0);
    lcd.printf("Kp=");
    lcd.locate(0,1);
    lcd.printf("Ki=");
    lcd.locate(8,1);
    lcd.printf("Kd=");
    
   
     //Inicio del ciclo
     
     while(1) {
           if (bot1.falling()) {            //----------------- 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;
               }
         
                        valor = Encoder.getPulses();    //------------- Asigna el valor de los pulsos del encoder a una variable llamada "valor"                                                
                       
                        switch(a) {
            
                        case 0:
                       
                        sp = sp + valor;                //------------- Asigna el valor del encoder al parámetro sp y tiene en cuenta el valor anterior
                        Encoder.reset();                //------------- Resetea el valor del encoder
                        if (sp<0){
                            sp=0;                       //------------- No se admite valores negativos
                            }
                        
                        lcd.locate(2,0);                //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD
                        lcd.printf("=    ",sp);
                        lcd.locate(3,0);
                        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;
                            
                            a=1;
                            led1=0;
                            wait(.15); 
                            led1=1;
                            Encoder.reset();            //------------- Resetea el valor del encoder
                            }
                        break;                           
                            
                                case 1:
                                
                                kp = kp + valor;        //------------- Asigna el valor del encoder al parámetro kp y tiene en cuenta el valor anterior
                                Encoder.reset();        //------------- Resetea el valor del encoder
                                if (kp<0){
                                kp=0;                   //------------- No se admite valores negativos
                                }
                        
                                lcd.locate(10,0);       //------------- Ubica e imprime el parámetro "sp" en la pantalla LCD
                                lcd.printf("=    ",kp);
                                lcd.locate(11,0);
                                lcd.printf("%i",kp);                        
                                wait(0.15);
                                   
                                    if (bot1.falling()) { //--------------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;
                                    wait(.15); 
                                    led1=1;
                                    Encoder.reset();      //------------- Resetea el valor del encoder
                                    }
                                break; 
                        
                                        case 2:
                                        
                                        ki = ki + valor;  //------------- Asigna el valor del encoder al parámetro ki y tiene en cuenta el valor anterior
                                        Encoder.reset();  //------------- Resetea el valor del encoder
                                        if (ki<0){
                                        ki=0;             //------------- No se admite valores negativos
                                        }
                                        
                                        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;
                                            
                                            a=3;
                                            led1=0;
                                            wait(.15); 
                                            led1=1;
                                            Encoder.reset();    //------------- Resetea el valor del encoder
                                            }
                                        break;
                        
                                                case 3:
                                                kd = kd + valor; //------------- Asigna el valor del encoder al parámetro kd y tiene en cuenta el valor anterior
                                                Encoder.reset(); //------------- Resetea el valor del encoder
                                                if (kd<0){
                                                kd=0;            //------------- No se admite valores negativos
                                                }
                                                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;
                                                    
                                                    a=0;
                                                    led1=0;
                                                    wait(.15); 
                                                    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;                
                                //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
      }