PID_encoder

Dependencies:   TextLCD mbed

main.cpp

Committer:
squinteroz
Date:
2015-10-28
Revision:
0:40599894595a
Child:
1:954623d16310

File content as of revision 0:40599894595a:

#include "mbed.h"
#include "TextLCD.h"// nos permite usar el lcd en nuestro comando 
#include "DebouncedIn.h" // esta libreria es útil para podeer declarar todas las teclas o botones que se posean 
#include "QEI.h" // nos permite  utilizar el encoder
TextLCD lcd(PTE0, PTE1, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7 Teclado
QEI wheel (PTD5, PTD0, NC, 100);
//asignamos el puerto a cada interruptor

DebouncedIn button1(PTC12); //Aumentar
DebouncedIn button2(PTC13); //Decrementar 
DebouncedIn button3(PTC16); //Pasar 
DebouncedIn button4(PTC17); //inicio

// asignamos los pines a la salida y a la entrada  (analógicas)

AnalogIn y(PTB1);   // Salida de la planta
AnalogOut u(PTE30); // Señal de control

//Salidas digitales utilziadas para la comprobación del funcionamiento 

DigitalOut led1(LED1); 
DigitalOut led2(LED2);
DigitalOut led3(LED3);
Timer t;// nos permite saber cuanto tiempo permanecen presionados los botones asociados al aumento y al decremento  
int j;
int i;
int C1=0x0F;  // inicializamos el lcd y el tipo de formato que este ultizará 
int sp=0,kp=0,kd=0,ki=0,p=1; // condiciones iniciales de los parametros de controlador. NOTA: la variable p indica en que se  parametro se desea intervenir (kp,ki,kd,sp, pasar e iniciar)
float  ap, ai, ad, err, med, err_v, pid=0;  // 

int main() {
    lcd.writeCommand(C1);
    lcd.printf(" Controlador PID\n");
    wait(2);
    lcd.cls();
    lcd.printf("    Equipo_5:\nEliana Mejia E.");
    wait(2);
    lcd.cls();
    lcd.printf("Sebastian Quintero Z. Sebastian Vergara P");
    wait(2);
    lcd.cls();
    lcd.printf("  Ingresar Parametros ");
    wait(2);
    
    retorno:   //Cuando se presiona por segunda vez el botón 4 se retorna a la interfaz de parametros para modificarlos
    p=1;
    lcd.cls();
    lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
    lcd.locate(8,0);
    lcd.printf("Kp=%d", kp);
    lcd.locate(0,1);
    lcd.printf("Ki=%d", ki);
    lcd.locate(8,1);
    lcd.printf("Kd=%d", kd);
    lcd.locate(0,0);
    lcd.printf("Sp=%d", sp);

    while(1) {
    
              led3 =1;
           if (button3.falling()) {   //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder)
              led3 =!led3;              
               ++j;
               }    
                                                
           if (j==0){      
               sp=sp+wheel.getPulses();
               wheel.reset();
                 if (sp>999){
                       sp=999;
                            } 
                 if (sp<0){
                       sp=0;
                          }   
               lcd.locate(2,0);
               lcd.printf("     ",sp);
               lcd.locate(2,0);
               lcd.printf("%d",sp);
               wait(0.2);
               
               if(button3.falling()){
                 j=1;
                 led3=0;
                 wait(0.3);
                 wheel.reset();   
                                    }   
                   
                     }
              
           if (j==1) {
               kp=kp+wheel.getPulses();
               wheel.reset();
                 if (kp>999){
                     kp=999;
                          }              
                 if (kp<0){
                     kp=0;
                          }   
               lcd.locate(10,0);
               lcd.printf("     ");
               lcd.locate(10,0);
               lcd.printf("%d",kp);
               wait(0.2);
               
               if(button3.falling()){
                 j=2;
                 led3=0;
                 wait(0.3);
                 wheel.reset();    
                                    }
                                           
                      }
              
           if (j==2) {
               ki=ki+wheel.getPulses();
               wheel.reset();
                 if (ki>999){
                     ki=999;
                          }              
                 if (ki<0){
                     ki=0;
                          }   
               lcd.locate(2,1);
               lcd.printf("     ");
               lcd.locate(2,1);
               lcd.printf("%d",ki);
               wait(0.2);
               
               if(button3.falling()){
                 j=3;
                 led3=0;
                 wait(0.3);
                 wheel.reset();
                                    }
                                    
                     }
                     
           if (j==3) {
               kd=kd+wheel.getPulses();
               wheel.reset();
                 if (kd>999){
                     kd=999;
                            }              
                 if (kd<0){
                     kd=0;
                          }   
               lcd.locate(10,1);
               lcd.printf("     ");
               lcd.locate(10,1);
               lcd.printf("%d",kd);
               wait(0.2);
               
               if(button3.falling()){
                 j=0;
                 led3=0;
                 wait(0.3);
                 wheel.reset();
                                    }
                                            
                       } 
            
           if (j==4) {
               j=0;
                     }                          
                     
           if (!button4){
           break;        //sale del bucle si pisan suiche4
                        }                  
              }          //cierro while(1)
//%---------------------------------------------------------------------   
      // accionamiento del botón asociado al inicio del programa realizado
       
  
           
           // 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",sp);
           lcd.locate(8,1);
           lcd.printf("Co%d",pid);
           wait(1); 
           
       
        
        p=0;
        if(p==0){
        lcd.cls();// limpia la pantalla y la localiza en (0,0) 
        lcd.printf("Iniciando PID\n");
        wait(1); 
        while(1){    //si se presiona por segunda vez es para modificar los parámetros 
           
        
        med=5*y.read(); // las constantes por la cuál se amplifica la señal dependen del sistema que se desee controlar y en donde lo deseamos limitar 
        err=sp-med; // error del sistema 
        ap = kp*err; 
        
        // se verifica que la accion integral no sea muy grande
        
           if(ai<5)
           {
              ai =(ki*err)+ai; //calculo de la integral del error
           }                     
           ad = kd*(err-err_v); //calculo de la acción derivativa
           pid = (ap+ai+ad);
           
           // se verifica que pid sea positivo **************************************
           if(pid<=0){pid=0;}
           
           // se verifica que pid sea menor o igual l valor maximo *****************
          
           if(pid>=5){pid=5;}
           u.write(pid); // enviamos el valor de PID  al puerto analógico  de salida 
           t.start();  
            
            if(t>=1)
            {
                    lcd.cls(); //limpiamos la pantalla del lcd
                    lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD en este caso modulo 1
                    lcd.locate(8,0);
                    lcd.printf("Err=%0.1f", err);
                    lcd.locate(0,1);
                    lcd.printf("Med=%0.1f", med);
                    lcd.locate(8,1);
                    lcd.printf("PID=%0.1f", pid);
                    lcd.locate(0,0);
                    lcd.printf("Sp=%d", sp);
                
                t.reset();
                
                } 
            }

     
        }     
    }