main.cpp

00001 #include "mbed.h"
00002 #include "DebouncedIn.h"
00003 #include "TextLCD.h"
00004 #include "QEI.h"
00005  
00006 AnalogIn Vin(PTB0);     //entrada de la planta
00007 AnalogOut Vout(PTE30);      //salida hacia la planta
00008  
00009 QEI wheel (PTA16, PTA17, NC, 24);   //distribucion de pines para el  encoder.
00010 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00011  
00012 DigitalOut led1(LED1);
00013 DigitalOut led2(LED2);
00014 DigitalOut led3(LED3);
00015 DigitalOut led4(LED4);
00016 DebouncedIn button1(PTC12);     // Aumento
00017 DebouncedIn button2(PTC13);     // Disminución
00018 DebouncedIn button3(PTC16);     // Cambio de variable
00019 DebouncedIn button4(PTC17);     // Reset  
00020 
00021 int C1=0x0E; // solo muestra el curzor
00022 int C2=0x18; // desplaza izquierda
00023 int C3=0x1A; // desplaza derecha
00024 int C4=0x0C; // quito cursor bajo
00025 int i; 
00026 int j,kp,ki,kd,sp,cont,medida,cont1,cont2,cont3;                                                      
00027 float pid, err, ap, ai, ad, err_v, spf,kif,kpf,kdf,med;
00028 
00029 
00030 int main() {
00031     
00032     lcd.writeCommand(C1);
00033     lcd.printf("Sp=0    Kp=0");
00034     lcd.locate(0,1);
00035     lcd.printf("Ki=0    Kd=0");
00036     lcd.locate(2,0);
00037     lcd.printf("=");
00038  
00039     while(1) {
00040       
00041      cont1=wheel.getPulses();           //inicia el encoder 
00042      if(cont1<0){
00043      wheel.reset();
00044         cont1=0; 
00045      }
00046             if(cont1!=cont2) {  
00047                                                 
00048             switch(i) {
00049             
00050                 case 0:
00051                
00052                     lcd.locate(2,0);
00053                     lcd.printf("=     ");
00054                     lcd.locate(3,0);
00055                     lcd.printf("%d",cont1);
00056                     sp=cont1;
00057                     
00058                     break;
00059                 case 1:
00060                
00061                     lcd.locate(10,0);
00062                     lcd.printf("=     ");
00063                     lcd.locate(11,0);
00064                     lcd.printf("%d",med);
00065                     kp=medida;
00066                     break;
00067                 case 2:
00068                     lcd.locate(2,1);
00069                     lcd.printf("=     ");
00070                     lcd.locate(3,1);
00071                     lcd.printf("%d",med);
00072                     ki=med;
00073                     break;
00074                 case 3:
00075                     lcd.locate(10,1);
00076                     lcd.printf("=     ");
00077                     lcd.locate(11,1);
00078                     lcd.printf("%d",med);
00079                     kd=med;
00080                     break;
00081             }
00082             cont2=cont1;
00083             
00084         }
00085                   
00086         
00087         if(button3.falling()) {         //al oprimir  el encoder
00088         wheel.reset();
00089             i++;
00090             if(i>3) {
00091                 i=0;
00092             }
00093             switch (i) {
00094                 case 0:
00095                     lcd.locate(2,0);
00096                     lcd.printf("=");
00097                     break;
00098                 case 1:
00099                     lcd.locate(10,0);
00100                     lcd.printf("=");
00101                     break;
00102                 case 2:
00103                     lcd.locate(2,1);
00104                     lcd.printf("=");
00105                     break;
00106                 case 3:
00107                     lcd.locate(10,1);
00108                     lcd.printf("=");
00109                     break;
00110             }
00111         }       
00112     
00113               
00114            if (button4.falling()){
00115                     lcd.locate(0,1);
00116                     lcd.printf("Ha salido del programa ");
00117            break;     
00118                }
00119                         
00120                                         }
00121         
00122     lcd.writeCommand(C4);
00123            led3=1;
00124            led3= !led3;
00125            lcd.cls();
00126            lcd.printf(" DATOS GUARDADOS!");   
00127            wait(0.8);
00128            lcd.cls();
00129            lcd.printf("INICIANDO EL PID");
00130            wait(0.2);           
00131            lcd.cls();
00132            lcd.printf("Er%d",err);
00133            lcd.locate(8,0);
00134            lcd.printf("Med%d",med);
00135            lcd.locate(0,1);
00136            lcd.printf("Sp%d",sp);
00137            lcd.locate(8,1);
00138            lcd.printf("pid%d",pid);
00139            wait(0.5);
00140            
00141            // CICLO PRINCIPAL CONTROLADOR PID
00142               while(1) {
00143                              
00144            med=Vin.read();
00145            medida=med*100;
00146            kpf=kp*0.001;
00147            kif=ki*0.001;
00148            kdf=kd*0.0001;
00149            err = (sp-medida);
00150            
00151            ap = kpf*err;         
00152            ai =(kif*err)+ai;             //calculo de la integral del error
00153             ad = kdf*(err-err_v);        //calculo de la accion derivativa
00154            err_v=err;                    //guarda el error
00155            pid = (ap+ai+ad);
00156            
00157                        
00158             if (pid > 99){
00159            pid=100;
00160            } 
00161            
00162                   
00163            if (pid <0){
00164            pid=0;
00165            } 
00166            
00167            wait(.5);
00168             
00169            
00170            
00171                     // se verifica que la accion integral no sea muy grande
00172            if (ai > 999){
00173            ai=1000;
00174            } 
00175           
00176            
00177            Vout=pid;
00178            
00179                    
00180            lcd.locate(2,0);
00181            lcd.printf("      "); 
00182            lcd.locate(0,0);
00183            lcd.printf("Er%2.2f",err);
00184            lcd.locate(10,0);
00185            lcd.printf("      ");
00186            lcd.locate(8,0);
00187            lcd.printf("Me%d",medida);
00188            lcd.locate(2,1);
00189            lcd.printf("      ");
00190            lcd.locate(0,1);
00191            lcd.printf("Sp%d",sp);
00192            lcd.locate(10,1);
00193            lcd.printf("      ");
00194            lcd.locate(8,1);
00195            lcd.printf("Pid%4.2f",pid);
00196            cont=0;
00197            }
00198    }
00199 
00200 
00201