Revision 1:f0fa90a6fb6e, committed 2015-10-27

Comitter:

demo71

Date:

Tue Oct 27 01:10:35 2015 +0000

Parent:

0:694446246c35

Commit message:

Control PID de un circuito RC con parametros agregados mediante un encoder de cuadratura y visualizado en LCD 16x2

Changed in this revision

diff -r 694446246c35 -r f0fa90a6fb6e QEI.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Tue Oct 27 01:10:35 2015 +0000
@@ -0,0 +1,1 @@
+http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa

diff -r 694446246c35 -r f0fa90a6fb6e main.cpp
--- a/main.cpp	Mon Sep 28 22:43:43 2015 +0000
+++ b/main.cpp	Tue Oct 27 01:10:35 2015 +0000
@@ -4,33 +4,37 @@
 #include "stdio.h"
 #include "TextLCD.h"
 #include "DebouncedIn.h" 
+#include "QEI.h"
 
 //Declaracion de entradas y salidas
-DebouncedIn Boton1(PTE20);  //Selector
-DebouncedIn Boton2(PTE21);  //Aumento
-DebouncedIn Boton3(PTE22);  //Disminuye
-DebouncedIn Boton4(PTE23);  //Confirma
+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
 
 //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 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;
-Timer tu;
-Timer td;
+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();
-    //escribimos un comando segun el manual del modulo LCD
     lcd.locate(1,0);
     lcd.printf("Kp=%d",Kp);
     lcd.locate(9,0);
@@ -38,7 +42,7 @@
     lcd.locate(1,1);
     lcd.printf("Kd=%d",Kd);
     lcd.locate(9,1);
-    lcd.printf("Sp=%d",Sp);
+    lcd.printf("Sp=%.1f",Sp);
     
     
     set_Kp:
@@ -51,49 +55,14 @@
     lcd.locate(1,1);
     lcd.printf("Kd=%d",Kd);
     lcd.locate(9,1);
-    lcd.printf("Sp=%d",Sp);
+    lcd.printf("Sp=%.1f",Sp);
     
     
-    //Kp=0;
     while(1){
          
-     if(Boton2.falling()){
-         Kp++;
-         tu.start();                  
-     }
-     //AUMENTO*10 **************
-     if(Boton2.rising()){
-         tu.stop();
-         tu.reset();
-     }
-     if(tu.read()>=2.0 && tu.read()<4.0){
-             Kp=Kp+10;
-             wait(0.3);
-     }
-     else if(tu.read()>=4.0){
-            Kp=Kp+100;
-            wait(0.3);
-     }
-     //*************************            
-     if(Boton3.falling()){
-         Kp--;
-         td.start();
-     }
-       //DISMINUYE*10 **************
-     if(Boton3.rising()){
-         td.stop();
-         td.reset();
-     }
-     if(td.read()>=2.0 && td.read()<4.0){
-             Kp=Kp-10;
-             wait(0.3);
-     }
-     else if(td.read()>=4.0){
-            Kp=Kp-100;
-            wait(0.3);
-     }
-     //************************* 
-     
+    Kp=Kp+wheel.getPulses();
+    wheel.reset();
+              
      if(Kp>=999){
             Kp=999;
      }
@@ -104,11 +73,12 @@
     lcd.locate(0,0);
     lcd.printf(">Kp=%d  ",Kp);
     
-    if(Boton1.falling()){
+    if(Boton2.falling()){
         goto set_Ki;
         }
         
-    if(Boton4.falling()){
+        
+    if(Boton1.falling()){
         goto PID;
     }
         
@@ -125,47 +95,13 @@
     lcd.locate(1,1);
     lcd.printf("Kd=%d",Kd);
     lcd.locate(9,1);
-    lcd.printf("Sp=%d",Sp);
+    lcd.printf("Sp=%.1f",Sp);
     
     //Ki=0;
     while(1){
          
-     if(Boton2.falling()){
-         Ki++;
-         tu.start();                  
-     }
-     //AUMENTO*10 **************
-     if(Boton2.rising()){
-         tu.stop();
-         tu.reset();
-     }
-     if(tu.read()>=2.0 && tu.read()<4.0){
-             Ki=Ki+10;
-             wait(0.3);
-     }
-     else if(tu.read()>=4.0){
-            Ki=Ki+100;
-            wait(0.3);
-     }
-     //*************************            
-     if(Boton3.falling()){
-         Ki--;
-         td.start();
-     }
-       //DISMINUYE*10 **************
-     if(Boton3.rising()){
-         td.stop();
-         td.reset();
-     }
-     if(td.read()>=2.0 && td.read()<4.0){
-             Ki=Ki-10;
-             wait(0.3);
-     }
-     else if(td.read()>=4.0){
-            Ki=Ki-100;
-            wait(0.3);
-     }
-     //************************* 
+     Ki=Ki+wheel.getPulses();
+     wheel.reset();
      
      if(Ki>=999){
             Ki=999;
@@ -177,11 +113,11 @@
     lcd.locate(8,0);
     lcd.printf(">Ki=%d  ",Ki);
     
-    if(Boton1.falling()){
+    if(Boton2.falling()){
         goto set_Kd;
         }
         
-    if(Boton4.falling()){
+    if(Boton1.falling()){
         goto PID;
     }
         
@@ -198,48 +134,14 @@
     lcd.locate(9,0);
     lcd.printf("Ki=%d",Ki);
     lcd.locate(9,1);
-    lcd.printf("Sp=%d",Sp);
+    lcd.printf("Sp=%.1f",Sp);
     
     
     //Kd=0;
     while(1){
          
-     if(Boton2.falling()){
-         Kd++;
-         tu.start();                  
-     }
-     //AUMENTO*10 **************
-     if(Boton2.rising()){
-         tu.stop();
-         tu.reset();
-     }
-     if(tu.read()>=2.0 && tu.read()<4.0){
-             Kd=Kd+10;
-             wait(0.3);
-     }
-     else if(tu.read()>=4.0){
-            Kd=Kd+100;
-            wait(0.3);
-     }
-     //*************************            
-     if(Boton3.falling()){
-         Kd--;
-         td.start();
-     }
-       //DISMINUYE*10 **************
-     if(Boton3.rising()){
-         td.stop();
-         td.reset();
-     }
-     if(td.read()>=2.0 && td.read()<4.0){
-             Kd=Kd-10;
-             wait(0.3);
-     }
-     else if(td.read()>=4.0){
-            Kd=Kd-100;
-            wait(0.3);
-     }
-     //************************* 
+     Kd=Kd+wheel.getPulses();
+     wheel.reset();
      
      if(Kd>=999){
             Kd=999;
@@ -251,11 +153,11 @@
     lcd.locate(0,1);
     lcd.printf(">Kd=%d  ",Kd);
     
-    if(Boton1.falling()){
+    if(Boton2.falling()){
         goto set_Sp;
     }
     
-    if(Boton4.falling()){
+    if(Boton1.falling()){
         goto PID;
     }
         
@@ -277,97 +179,63 @@
     //Sp=0;
     while(1){
          
-     if(Boton2.falling()){
-         Sp++;
-         tu.start();                  
-     }
-     //AUMENTO*10 **************
-     if(Boton2.rising()){
-         tu.stop();
-         tu.reset();
-     }
-     if(tu.read()>=2.0 && tu.read()<4.0){
-             Sp=Sp+10;
-             wait(0.3);
-     }
-     else if(tu.read()>=4.0){
-            Sp=Sp+100;
-            wait(0.3);
-     }
-     //*************************            
-     if(Boton3.falling()){
-         Sp--;
-         td.start();
-     }
-       //DISMINUYE*10 **************
-     if(Boton3.rising()){
-         td.stop();
-         td.reset();
-     }
-     if(td.read()>=2.0 && td.read()<4.0){
-             Sp=Sp-10;
-             wait(0.3);
-     }
-     else if(td.read()>=4.0){
-            Sp=Sp-100;
-            wait(0.3);
-     }
-     //************************* 
+     Sp=Sp+0.1*wheel.getPulses();
+     wheel.reset();
      
-     if(Sp>=999){
-            Sp=999;
+     if(Sp>=3.3){
+            Sp=3.3;
      }
      else if (Sp<=0){
              Sp=0;
      }
      
     lcd.locate(8,1);
-    lcd.printf(">Sp=%d  ",Sp);
+    lcd.printf(">Sp=%.1f  ",Sp);
     
-    if(Boton1.falling()){
+    if(Boton2.falling()){
         goto set_Kp;
     }
     
-    if(Boton4.falling()){
+    if(Boton1.falling()){
         goto PID;
     }
                
     }
     
     PID:
+    
     lcd.cls();
-    
-    //Sp=2;
-    //Kp=(float)12.105;                     
-    //Ki=(float)5.32;
-    //Kd=(float)2;
     while(1){
     
     yT=vcond.read()*3.3;
-    eT=Sp-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;                        //Guardar variables
-        eT0=eT; 
-        control=(float)uT/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.
         
+        ekm2=ekm1;
+        ekm1=ek;
+        ukm1=uk;
         lcd.locate(0,0);
-        lcd.printf("Error=%.2f ",eT);
+        lcd.printf("Error=%.2f ",ek);
         lcd.locate(0,1);
         lcd.printf("Y_act=%.2f",yT);
-        wait(0.2);
+        wait(Ts); // Muestreos
     }
-    //lcd.locate(3,0);    
-    //lcd.writeCommand(C1);
- 
-
-
+    
 }
 
  
\ No newline at end of file