Diff: main.cpp

Revision:

0:43669293f24e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Nov 13 14:10:06 2015 +0000
@@ -0,0 +1,247 @@
+// PID con encoder y boton
+ 
+#include "mbed.h"
+#include "stdio.h"
+#include "TextLCD.h"
+#include "DebouncedIn.h" 
+ #include "QEI.h"
+ 
+//Declaracion de entradas y salidas
+DebouncedIn Boton1(PTA1);  //Boton para confirmar
+DebouncedIn Boton2(PTC9);  //Boton para pasar
+ 
+//Configuracion encoder
+QEI wheel (PTD5, PTD0, NC, 100);
+
+ 
+PwmOut control(PTE29);
+AnalogIn vsal(PTB0);
+ 
+TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7
+ 
+//codigos movimiento del cursor LCD
+int C2=0x18; // desplaza izquierda
+int C3=0x1A; // desplaza derecha
+int C4=0x0C; // quito cursor bajo
+int C1=0x0F; // solo muestra el cursor
+ 
+int Kp=0,Ki=0,Kd=0,Sp=0,p,i;
+float rT,eT,iT,dT,yT,uT,iT0=0,eT0=0,iT_1=0,eT_1=0,spf=0;
+Timer tu;
+Timer td;
+ 
+int main() {
+    
+    lcd.cls();
+     lcd.locate(0,0);
+    lcd.printf("PID_ENCODER");
+    wait(1);
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Omar Torres");
+    lcd.locate(0,1);
+    lcd.printf("Sebastian Zapata");
+    wait(2);
+    lcd.cls();
+   
+    //comando basado en el manual del LCD
+    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=%d",Sp);
+    
+    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=%d",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;
+    }
+    }
+    
+    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=%d",Sp);
+    
+    //Ki=0;
+    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;
+    }
+    }
+    
+    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=%d",Sp);
+    
+    //Kd=0;
+    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;
+    }
+    }
+    
+    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);
+            
+    //Sp=0;
+    while(1){
+         
+    Sp=Sp+wheel.getPulses();
+     wheel.reset();
+     
+     if(Sp>=330){
+            Sp=330;
+     }
+     else if (Sp<=0){
+             Sp=0;
+     }
+     
+    lcd.locate(8,1);
+    lcd.printf("|Sp=%d  ",Sp);
+    
+    if(Boton2.falling()){
+        goto set_Kp;
+    }
+    
+    if(Boton1.falling()){
+        goto PID;
+    }
+    }
+    
+    PID:
+    lcd.cls();
+    
+    lcd.locate(0,0);
+    lcd.printf("Guardando..");
+    wait(0.6);
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Guardando....");
+    wait(0.6);
+    lcd.cls();
+    lcd.locate(0,0);
+    lcd.printf("Guardando......");
+    wait(0.6);
+    lcd.cls();
+    
+    spf=(float)Sp/100;   //convirtiendo de decivoltios a voltios
+    while(1){
+    
+    yT=vsal.read()*3.3;
+    eT=spf-yT;
+    iT=Ki*eT+iT0;        //Accion Integral
+    dT=Kd*(eT-eT0);      //Accion Derivativa
+    uT=iT+Kp*eT+dT;
+    if (uT>3.3) {        //Salida PID si es mayor que el MAX 
+        uT=3.3;}                          
+     else if (uT<0){      //Salida PID si es menor que el MIN 
+        uT=0;                         
+        } 
+        iT0=iT;                        //Guarda las variables
+        eT0=eT; 
+        control=(float)uT/3.3;
+        
+        lcd.locate(0,0);
+        lcd.printf("Err=%.2f ",eT);
+        lcd.locate(0,1);
+        lcd.printf("Sal=%.2f",yT);
+        wait(0.2);
+    }
+}
+ 
\ No newline at end of file