File content as of revision 2:921bc4072c83:

// Hello World! for the TextLCD
 
#include "mbed.h"
#include "stdio.h"
#include "TextLCD.h"
#include "DebouncedIn.h" 
#include "QEI.h"

//Declaracion de entradas y salidas
DebouncedIn Boton1(PTE20);  //Boton para confirmar
DebouncedIn Boton2(PTE21);  //Boton para confirmar

//Configuracion encoder
QEI wheel (PTD5, PTD0, NC, 100);

PwmOut control(PTE29);
AnalogIn vcond(PTB0);

//Configuracion pantalla LCD 
TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7

//Variables y parametros del control PID
int Kp=0,Ki=0,Kd=0;
float Sp=0;
float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.1; //Ts Tiempo Muestreo
float yT,uk=0,ukm1=0;



int main() {
    
    lcd.cls();
    lcd.locate(1,0);
    lcd.printf("Kp=%d",Kp);
    lcd.locate(9,0);
    lcd.printf("Ki%d=",Ki);
    lcd.locate(1,1);
    lcd.printf("Kd=%d",Kd);
    lcd.locate(9,1);
    lcd.printf("Sp=%.1f",Sp);
    
    //Configuracion de constante Kp
    set_Kp:
    lcd.cls();
    lcd.locate(0,0);
    lcd.printf(">Kp=   ");
    
    lcd.locate(9,0);
    lcd.printf("Ki=%d",Ki);
    lcd.locate(1,1);
    lcd.printf("Kd=%d",Kd);
    lcd.locate(9,1);
    lcd.printf("Sp=%.1f",Sp);
    
    
    while(1){
         
    Kp=Kp+wheel.getPulses();
    wheel.reset();
              
     if(Kp>=999){
            Kp=999;
     }
     else if (Kp<=0){
             Kp=0;
     }
     
    lcd.locate(0,0);
    lcd.printf(">Kp=%d  ",Kp);
    
    if(Boton2.falling()){
        goto set_Ki;
        }
        
        
    if(Boton1.falling()){
        goto PID;
    }
        
    }
    
    //Configuracion de Ki
    set_Ki:
    
    lcd.cls();
    lcd.locate(8,0);
    lcd.printf(">Ki=   ");
    
    lcd.locate(1,0);
    lcd.printf("Kp=%d",Kp);
    lcd.locate(1,1);
    lcd.printf("Kd=%d",Kd);
    lcd.locate(9,1);
    lcd.printf("Sp=%.1f",Sp);
    
    while(1){
         
     Ki=Ki+wheel.getPulses();
     wheel.reset();
     
     if(Ki>=999){
            Ki=999;
     }
     else if (Ki<=0){
             Ki=0;
     }
     
    lcd.locate(8,0);
    lcd.printf(">Ki=%d  ",Ki);
    
    if(Boton2.falling()){
        goto set_Kd;
        }
        
    if(Boton1.falling()){
        goto PID;
    }
        
    }
    
    //Configuracion de Kd
    set_Kd:
    
    lcd.cls();
    lcd.locate(0,1);
    lcd.printf(">Kd=   ");
    
    lcd.locate(1,0);
    lcd.printf("Kp=%d",Kp);
    lcd.locate(9,0);
    lcd.printf("Ki=%d",Ki);
    lcd.locate(9,1);
    lcd.printf("Sp=%.1f",Sp);
    
    
    
    while(1){
         
     Kd=Kd+wheel.getPulses();
     wheel.reset();
     
     if(Kd>=999){
            Kd=999;
     }
     else if (Kd<=0){
             Kd=0;
     }
     
    lcd.locate(0,1);
    lcd.printf(">Kd=%d  ",Kd);
    
    if(Boton2.falling()){
        goto set_Sp;
    }
    
    if(Boton1.falling()){
        goto PID;
    }
        
    }
    
    //Configuracion de Sp
    set_Sp:
    
    lcd.cls();
    lcd.locate(8,1);
    lcd.printf(">Sp=   ");
    
    lcd.locate(1,0);
    lcd.printf("Kp=%d",Kp);
    lcd.locate(9,0);
    lcd.printf("Ki=%d",Ki);
    lcd.locate(1,1);
    lcd.printf("Kd=%d",Kd);
            
   
    while(1){
         
     Sp=Sp+0.1*wheel.getPulses();
     wheel.reset();
     
     if(Sp>=3.3){
            Sp=3.3;
     }
     else if (Sp<=0){
             Sp=0;
     }
     
    lcd.locate(8,1);
    lcd.printf(">Sp=%.1f  ",Sp);
    
    if(Boton2.falling()){
        goto set_Kp;
    }
    
    if(Boton1.falling()){
        goto PID;
    }
               
    }
    
    //Inicia algoritmo del PID
    PID:
    
    lcd.cls();
    while(1){
    
    yT=vcond.read()*3.3;
    ek=Sp-yT;
    
    //Conversion de parametros PID Continuo a parametros de PID Digital 
    //ESTO ES UN PID DIGITAL Y LOS PARAMETROS AGREGADOS SON PARA UN PID CONTINUO!!
    
    K1=Kp+(Ki/2)*Ts+(Kd/Ts);
    K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts);
    K3=Kd/Ts;
    
    uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control
    
    if (uk>3.3) {        //Salida PID si es mayor que el MAX 
        uk=3.3;}                          
     else if (uk<0){      //Salida PID si es menor que el MIN 
        uk=0;                         
        }
                 
        control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta.
        
        //Actualizacion de datos para el siguiente paso
        ekm2=ekm1; 
        ekm1=ek;
        ukm1=uk;
        
        //Mostrando error y salida actual
        lcd.locate(0,0);
        lcd.printf("Error=%.2f ",ek);
        lcd.locate(0,1);
        lcd.printf("Y_act=%.2f",yT);
        wait(Ts); //Tiempo de muestreo
    }
    
}