main.cpp

00001 //Software para realizar control PID en tarjeta Freescale KL25Z con ENCODER  y Display 16x2
00002 
00003 #include "mbed.h"
00004 #include "DebouncedIn.h"
00005 #include "TextLCD.h"
00006 #include "QEI.h" 
00007 
00008 AnalogIn Vin(PTC2);
00009 // AnalogOut Vout(PTE30);
00010 
00011 AnalogIn y(PTB0);       // Revizar su funcion
00012 AnalogOut u(PTE30);     // Revizar su funcion
00013 
00014 
00015 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00016 QEI wheel (PTA1, PTA2, NC, 100);
00017 
00018 DigitalOut led1(LED1);
00019 DigitalOut led2(LED2);
00020 DigitalOut led3(LED3);
00021 DebouncedIn button1(PTC12);
00022 DebouncedIn button2(PTC13);
00023 DebouncedIn button3(PTD4);
00024 DebouncedIn button4(PTA12);
00025 
00026 //int C1=0x0E; // solo muestra el curzor    
00027 int C1=0x0F;        // PORQUE????
00028 int C2=0x18; // desplaza izquierda
00029 int C3=0x1A; // desplaza derecha
00030 int C4=0x0C; // quito cursor bajo
00031 
00032 int i;                  // Indice de la variable
00033 int j;                  // Variable controla cambio 4 posiciones
00034 int kp, ki, kd, sp, err, med, err_v, ap, ai, ad, pid;
00035 int cont, sal, yr , cycle; 
00036 
00037 float pidn;
00038 
00039 int main() {
00040     lcd.cls();
00041     lcd.writeCommand(C1);
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()){   //Posición Encoder Boton 3.
00056               led3 =!led3;              
00057                ++j;
00058                                   }    
00059                                                 
00060            if (j==0){      
00061                sp=sp+wheel.getPulses();
00062                wheel.reset();
00063                  if (sp>1000){
00064                        sp=1000;
00065                             } 
00066                  if (sp<0){
00067                        sp=0;
00068                           }   
00069                lcd.locate(3,0);
00070                lcd.printf("     ");      //En futuro probrar sin Sp.
00071                lcd.locate(3,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>1000){
00088                      kp=1000;
00089                           }              
00090                  if (kp<0){
00091                      kp=0;
00092                           }   
00093                lcd.locate(11,0);
00094                lcd.printf("     ");
00095                lcd.locate(11,0);
00096                lcd.printf("%d",kp);
00097                wait(0.3);
00098                
00099                if(button3.falling()){
00100                  j=2;
00101                  led3=0;
00102                  wait(0.3);
00103                  wheel.reset();    
00104                                     }
00105                             }
00106               
00107            if (j==2) {
00108                ki=ki+wheel.getPulses();
00109                wheel.reset();
00110                  if (ki>1000){
00111                      ki=1000;
00112                           }              
00113                  if (ki<0){
00114                      ki=0;
00115                           }   
00116                lcd.locate(3,1);
00117                lcd.printf("     ");
00118                lcd.locate(3,1);
00119                lcd.printf("%d",ki);
00120                wait(0.3);
00121                
00122                if(button3.falling()){
00123                  j=3;
00124                  led3=0;
00125                  wait(0.3);
00126                  wheel.reset();
00127                                     }
00128                                     
00129                      }
00130                      
00131            if (j==3) {
00132                kd=kd+wheel.getPulses();
00133                wheel.reset();
00134                  if (kd>1000){
00135                      kd=1000;
00136                             }              
00137                  if (kd<0){
00138                      kd=0;
00139                           }   
00140                lcd.locate(11,1);
00141                lcd.printf("     ");
00142                lcd.locate(11,1);
00143                lcd.printf("%d",kd);
00144                wait(0.3);
00145                
00146                if(button3.falling()){
00147                  j=4;       // CAMBIADO CERO POR 4.
00148                  led3=0;
00149                  wait(0.3);
00150                  wheel.reset();
00151                                     }
00152                                             
00153                        } 
00154             
00155            if (j==4) {
00156                j=0;
00157                      }                          
00158                      
00159            if (!button4){
00160            break;        //sale del bucle si pisan suiche4
00161                         }                  
00162               }          //cierro while(1)
00163 
00164 
00165 ///////////////////////////////////////////////////////////////////////////////////////////
00166               
00167               
00168           lcd.writeCommand(C4);//escribimos un comando segun el manual del modulo LCD para quitar cursor bajo
00169            lcd.cls(); //borra la pantalla
00170            lcd.printf("Guardando '_'"); 
00171            wait(2);
00172            
00173            // Se imprimen los parches del control.
00174            
00175            lcd.cls();
00176            lcd.printf("Err %d",err);
00177            lcd.locate(8,0);
00178            lcd.printf("Med %d",med);
00179            lcd.locate(0,1);
00180            lcd.printf("Sp %d",sp);
00181            lcd.locate(8,1);
00182            lcd.printf("Co %d",pid);
00183            wait(1);
00184            
00185            
00186            // CICLO PRINCIPAL CONTROLADOR PID
00187            
00188            while(1) {
00189            med=999*y.read();                   //leer puerto analogo y asignar a med
00190            err = (sp-med);
00191            ap = kp*err;
00192            
00193            // se verifica que la accion integral no sea muy grande
00194            if(ai<1000){
00195            ai =(ki*err)+ai;    //calculo de la integral del error
00196                       }
00197                       
00198            ad = kd*(err-err_v); //calculo de la accion derivativa
00199            pid = (ap+ai+ad);
00200            
00201            // PID siempre positivo.
00202            if(pid<=0){
00203               pid=0;
00204                      }
00205            // Limite máximo PID 
00206            if (pid > 9000){
00207                pid=9000;
00208                          } 
00209            //se muestran las variables******************************************
00210            
00211            wait(0.3);
00212            
00213            lcd.locate(3,0);
00214            lcd.printf("    ");
00215            lcd.locate(3,0);
00216            lcd.printf("%d",err);
00217            
00218            lcd.locate(12,0);
00219            lcd.printf("    ");
00220            lcd.locate(12,0);
00221            lcd.printf("%d",med);
00222            
00223            lcd.locate(3,1);
00224            lcd.printf("    ");
00225            lcd.locate(3,1);
00226            lcd.printf("%d",sp);
00227            
00228            lcd.locate(12,1);
00229            lcd.printf("    ");
00230            lcd.locate(12,1);
00231            lcd.printf("%d",pid);
00232            
00233            
00234            //Normalizacion de la salida
00235            pidn=pid/999;
00236            //  se envia el valor pid a puerto analogico de salida (D/A) **************
00237            u.write(pidn);
00238            err_v = err; 
00239            //  se repite el ciclo
00240            }
00241            
00242            
00243               
00244         }