main.cpp

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