main.cpp

00001 #include "mbed.h"
00002 #include "DebouncedIn.h"
00003 #include "TextLCD.h"
00004 #include "QEI.h" 
00005 
00006 
00007 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00008 QEI wheel (PTA1, PTA2, NC, 50);
00009 
00010 AnalogIn y(PTC2);
00011 AnalogOut u(PTE30);
00012 
00013 DigitalOut led1(LED1);
00014 DigitalOut led2(LED2);
00015 DigitalOut led3(LED3);
00016 DebouncedIn button1(PTC7);
00017 DebouncedIn button2(PTC13);
00018 DebouncedIn button3(PTC0);
00019 DebouncedIn button4(PTC17);
00020 
00021       //codigos movimiento del curzor
00022       //18 para izquierda
00023       //1A para derecha
00024 
00025 //int C1=0x0E; // solo muestra el curzor
00026 int C2=0x18; // desplaza izquierda
00027 int C3=0x1A; // desplaza derecha
00028 int C4=0x0C; // quito cursor bajo
00029 int C1=0x0F;
00030 
00031 int i; // indice de la variable
00032 int j; //variable controla cambio 4 posiciones
00033 int kp, ki, kd, sp, med2, yr, cycle, c; 
00034 float pid, med, ap, ai, ad, err, err_v;
00035 
00036 int main() {
00037     lcd.cls();
00038     lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD
00039     lcd.locate(0,0);
00040     lcd.printf("Sp%d",sp);
00041     lcd.locate(8,0);
00042     lcd.printf("Kp%d",kp);
00043     lcd.locate(0,1);
00044     lcd.printf("Ki%d",ki);
00045     lcd.locate(8,1);
00046     lcd.printf("Kd%d",kd);
00047   
00048         
00049     while(1) {
00050     
00051               led3 =1;
00052            if (button3.falling()) {   //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder)
00053               led3 =!led3;              
00054                ++j;
00055                }    
00056                                                 
00057            if (j==0){      
00058                sp=sp+wheel.getPulses();
00059                wheel.reset();
00060                  if (sp>999){
00061                        sp=999;
00062                             } 
00063                  if (sp<0){
00064                        sp=0;
00065                           }   
00066                lcd.locate(2,0);
00067                lcd.printf("     ",sp);
00068                lcd.locate(2,0);
00069                lcd.printf("%d",sp);
00070                wait(0.2);
00071                
00072                if(button3.falling()){
00073                  j=1;
00074                  led3=0;
00075                  wait(0.3);
00076                  wheel.reset();   
00077                                     }   
00078                    
00079                      }
00080               
00081            if (j==1) {
00082                kp=kp+wheel.getPulses();
00083                wheel.reset();
00084                  if (kp>999){
00085                      kp=999;
00086                           }              
00087                  if (kp<0){
00088                      kp=0;
00089                           }   
00090                lcd.locate(10,0);
00091                lcd.printf("     ");
00092                lcd.locate(10,0);
00093                lcd.printf("%d",kp);
00094                wait(0.2);
00095                
00096                if(button3.falling()){
00097                  j=2;
00098                  led3=0;
00099                  wait(0.3);
00100                  wheel.reset();    
00101                                     }
00102                                            
00103                       }
00104               
00105            if (j==2) {
00106                ki=ki+wheel.getPulses();
00107                wheel.reset();
00108                  if (ki>999){
00109                      ki=999;
00110                           }              
00111                  if (ki<0){
00112                      ki=0;
00113                           }   
00114                lcd.locate(2,1);
00115                lcd.printf("     ");
00116                lcd.locate(2,1);
00117                lcd.printf("%d",ki);
00118                wait(0.2);
00119                
00120                if(button3.falling()){
00121                  j=3;
00122                  led3=0;
00123                  wait(0.3);
00124                  wheel.reset();
00125                                     }
00126                                     
00127                      }
00128                      
00129            if (j==3) {
00130                kd=kd+wheel.getPulses();
00131                wheel.reset();
00132                  if (kd>999){
00133                      kd=999;
00134                             }              
00135                  if (kd<0){
00136                      kd=0;
00137                           }   
00138                lcd.locate(10,1);
00139                lcd.printf("     ");
00140                lcd.locate(10,1);
00141                lcd.printf("%d",kd);
00142                wait(0.2);
00143                
00144                if(button3.falling()){
00145                  j=0;
00146                  led3=0;
00147                  wait(0.3);
00148                  wheel.reset();
00149                                     }
00150                                             
00151                        } 
00152             
00153            if (j==4) {
00154                j=0;
00155                      }                          
00156                      
00157            if (!button4){
00158            break;        //sale del bucle si pisan suiche4
00159                         }                  
00160               }          //cierro while(1)
00161 //%---------------------------------------------------------------------              
00162               
00163               
00164            lcd.writeCommand(C1);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
00165            lcd.cls(); //borra la pantalla
00166            lcd.printf("GUARDAMOS \nVALORES |m|"); 
00167            wait(2);
00168            lcd.cls();
00169            lcd.printf(" INICIA EL PID");
00170            wait(2);
00171            lcd.cls();
00172            
00173            // se imprimen los parches del control  *****************************************
00174            lcd.locate(0,0);
00175            lcd.printf("Er%d",err);
00176            lcd.locate(8,0);
00177            lcd.printf("Me%d",med2);
00178            lcd.locate(0,1);
00179            lcd.printf("Sp%d",sp);
00180            lcd.locate(8,1);
00181            lcd.printf("Con%d",pid);
00182            //wait(3); 
00183            
00184            
00185            // CICLO PRINCIPAL CONTROLADOR PID
00186            
00187            while(1) {
00188            //leer puerto analogo y asignar a med
00189            med=y.read();       
00190            med2=med*100;                   
00191            err=(sp-med2);
00192            
00193            float kpf;
00194            kpf=kp*0.001;
00195            ap = kpf*err;
00196            float kif;
00197            kif=ki*0.001;
00198            ai =(kif*err)+ai;
00199            float kdf;
00200            kdf=kd*0.0001;
00201            ad = kdf*(err-err_v);
00202            err_v=err;
00203            pid = (ap+ai+ad);
00204            
00205            // se verifica que pid sea menor o igual la valor maximo *****************
00206            if (pid > .99999){
00207              pid=1;
00208                             } 
00209            // se verifica que pid sea positivo **************************************
00210            if (pid <0){
00211              pid=0;
00212                       } 
00213            // se verifica que la accion integral no sea muy grande
00214            if(ai>0.5){
00215              ai =0.5;    
00216                      }
00217            
00218            u=pid;
00219            
00220            //****se muestran las variables******************************************
00221           if(c>600){           
00222            lcd.locate(2,0);
00223            lcd.printf("      "); 
00224            lcd.locate(0,0);
00225            lcd.printf("Er%2.2f",err);
00226            lcd.locate(10,0);
00227            lcd.printf("      ");
00228            lcd.locate(8,0);
00229            lcd.printf("Me%d",med2);
00230            lcd.locate(2,1);
00231            lcd.printf("      ");
00232            lcd.locate(0,1);
00233            lcd.printf("Sp%d",sp);
00234            lcd.locate(10,1);
00235            lcd.printf("      ");
00236            lcd.locate(8,1);
00237            lcd.printf("Con%4.3f",pid);
00238            c=0;
00239            }
00240            else 
00241            c++;          
00242            
00243            }
00244            
00245            
00246             
00247 }