File content as of revision 0:fcae7980d3cf:

#include "mbed.h"
#include "TextLCD.h"
#include "DebouncedIn.h"
#include "QEI.h"

TextLCD lcd(PTC10, PTC11, PTC12, PTC13, PTC16, PTC17); // rs, e, d4-d7.. Nota: No se quiere función de lectura, luego ese puerto a tierra.
//lectura=1, escritura=0
QEI leftQei(PTD6, PTD7, NC, 624);
//Encoder Sp, Kp, Ki, Kd
PwmOut rled(LED_GREEN);

DebouncedIn next(PTE5);
AnalogIn Ain(PTC2);
AnalogOut Aout(PTE30);

int C1=0x0F; 
int i,j,k,l;
float ap,ai,ad;

float p; // set point
float s=0; // salida planta
float h=0; // entrada planta
float m=0; // Error
float mv=0;
float g=0;
float x=0;

int main() {

int c;
c=0;

lcd.cls();     
lcd.locate(0,0);
lcd.printf("sp:%d",i);
lcd.locate(8,0);
lcd.printf("kp:%d",j);
lcd.locate(0,1);
lcd.printf("ki:%d",k);
lcd.locate(8,1);
lcd.printf("kd:%d",l);

//SP

while (1){

if (next.falling()){    
        c++;
        }          

    //Salto a SP
    if (c==0){
        
        lcd.locate(3,0);
        lcd.putc(0xFE);                
        lcd.writeCommand(C1);
        lcd.locate(3,0);
        lcd.printf("%d",i);
        
            while(c==0){  
              
            //espacio de trabajo
                        
                i=leftQei.getPulses();
                //
                
                             
                lcd.locate(3,0);
                lcd.printf("%i",leftQei.getPulses());
                wait(0.2);          
                                   
           
            //fin espacio de trabajo            
            
            if(next.falling()){
                c++;
                }                                                             
                }
                } 
              
        
    //Salto a Kp
    if (c==1){
        
        lcd.locate(11,0);
        lcd.putc(0xFE);                
        lcd.writeCommand(C1);
        lcd.locate(11,0);
        lcd.printf("%d",j);
        
            while(c==1){
            
            //Work space
            
                 j=leftQei.getPulses();
                 
            //
                                             
                lcd.locate(11,0);
                lcd.printf("%i",leftQei.getPulses());
                wait(0.2); 
           
            //End work space            
              
             if(next.falling()){
                c++;
                }                                                             
                }
                }
              
    //Salto a Kp
    if (c==2){
        
        lcd.locate(3,1);
        lcd.putc(0xFE);                
        lcd.writeCommand(C1);
        lcd.locate(3,1);
        lcd.printf("%d",k);
        
            while(c==2){ 
            
            //Work space
            
                k=leftQei.getPulses();
                 
            //
                                             
                lcd.locate(3,1);
                lcd.printf("%i",leftQei.getPulses());
                wait(0.2); 
                
            //End work space  

             if(next.falling()){
                c++;
                }                                                             
                }
                }        

    //Salto a Kp
    if (c==3){
        
        lcd.locate(11,1);
        lcd.putc(0xFE);                
        lcd.writeCommand(C1);
        lcd.locate(11,1);
        lcd.printf("%d",l);
        
            while(c==3){
            
            //Work space
            
                l=leftQei.getPulses();
                 
            //
                                             
                lcd.locate(11,1);
                lcd.printf("%i",leftQei.getPulses());
                wait(0.2); 
                
           
            //End work space                           
             
             if(next.falling()){
                c++;
                                }                                                             
                }
                }   

    //          
                 if(c==4 || c>4){
                c=0;
                                }                                                                                  
                         
}

//Borrar

lcd.cls();
lcd.printf("Er%d",m);
lcd.locate(8,0);
lcd.printf("Me%d",s);
lcd.locate(0,1);
lcd.printf("Sp%d",i);
lcd.locate(8,1);
lcd.printf("Co%d",x);
wait(5);

//Ahora empieza el PID

p=i/100;

while(1) {
        wait(0.3);
              
        if(p<0.999){
       
        s=Ain;
        m=(p-s);
        //n=m*100;
        ap=j*m;
        ai=(k*m)+ai;
        
        if (ai>999){
        ai=998;
                   }
        
        ad=l*(m-mv);
        h=ap+ai+ad;
        x=h/100;
        g=(h*3.3/100);
        
        if (h<0){
            h=0;
                }
                

                        
        Aout=g;
        wait(0.22);
 
        
        lcd.cls();
        lcd.printf("Er:%.3f",m);
        lcd.locate(8,0);
        lcd.printf("Me:%.3f",s);
        lcd.locate(0,1);
        lcd.printf("Sp:%.2f",p);
        lcd.locate(8,1);
        lcd.printf("Co:%.3f",x);
        }


        mv=m;

        }
}