main.cpp

00001 #include "mbed.h"
00002 #include "TextLCD.h"
00003 #include "QEI.h" 
00004 #include "DebouncedIn.h"
00005 
00006 PwmOut Pwm1(PTA5);//Definicion de las Salidas.
00007 PwmOut Pwm2(PTD4);//Definicion de las Salidas.D
00008 DigitalOut led(LED1);
00009 DigitalOut  led2(LED2);
00010 DigitalOut led3(LED3);
00011 DebouncedIn button3(PTC16);
00012 DebouncedIn button4(PTC17);
00013 
00014 Serial pc(USBTX, USBRX, "pc");
00015 QEI wheel (PTD5, PTD0, NC, 24);
00016 AnalogIn y(PTB1);
00017 AnalogOut u(PTE30);
00018 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00019 
00020 int C4=0x0C; // quito cursor bajo
00021 
00022 float PI,PIant;
00023 float  medd;
00024 float sal;
00025 int i; // indice de la variable
00026 int j; //variable controla cambio 4 posiciones
00027 float kp=60, ki=0.26, kd=0, Sp,sp,Set=28,T;
00028 float Delta1=0,Delta2=0,Delta3=0,Delta22,Delta33;
00029 float yr, ap, ai, ad, err, med, err_v, cycle, pid,pid2;
00030 int b=0; 
00031 float pidn;
00032 
00033 float f=100;         //Frecuencia del PWM
00034 float pp=1/f;       //Perioo del PWM
00035 Timer t;
00036 
00037 int Fs=10;
00038 float Ts=0.1;
00039 float inte;
00040 float intek;
00041 
00042 
00043 int PWMmodule1(float pp,float Dd)
00044 {
00045         Pwm1.period(pp);
00046         Pwm1.write(Dd);  
00047         wait(0.01);
00048 } 
00049 
00050 int PWMmodule2(float pp,float Dd)
00051 {
00052         Pwm2.period(pp);
00053         Pwm2.write(Dd);
00054         wait(0.01); 
00055 } 
00056 int main() 
00057 {
00058     
00059     led=1;
00060     led2=0;
00061     medd=3.705503686*100*y.read();
00062     sp=medd;
00063     Sp=sp;
00064     inte=Set-medd;
00065     PIant=0;
00066     lcd.cls();
00067     //Valores Constantes, que se deben inicializar.
00068     lcd.locate(0,0);
00069     lcd.printf("Sp:%.1f",Set);
00070     lcd.locate(8,0);
00071     lcd.printf("T1:%.1f",Delta1);
00072     lcd.locate(0,1);
00073     lcd.printf("T2:%.1f",Delta2);
00074     lcd.locate(8,1);
00075     lcd.printf("T3:%.1f",Delta3);
00076     
00077     while(1)
00078     {
00079         led3 =1;
00080         if (button3.falling()) 
00081         {   //INCREMENTA POSICION DEL MENU CON BOTON 3 (Switche encoder)
00082             led3 =!led3;              
00083             ++j;
00084         }
00085       
00086         if(j==0)
00087         {
00088             Set=Set+wheel.getPulses();
00089             wheel.reset();
00090             if (Set>999)
00091             {
00092                 Set=999;
00093             } 
00094             if (Set<0)
00095             {
00096                 Set=0;
00097             }
00098             lcd.locate(3,0);
00099             lcd.printf("     ",Set);
00100             lcd.locate(3,0);
00101             lcd.printf("%.1f",Set);
00102             wait(0.2);
00103             
00104             if(button3.falling())
00105             {
00106             j=1;
00107             led3=0;
00108             wait(0.3);
00109             wheel.reset();  
00110             }
00111             
00112         }
00113         if(j==1)
00114         {
00115             Delta1=Delta1+wheel.getPulses();
00116             wheel.reset();
00117             if (Delta1>12000)
00118             {
00119                 Delta1=12000;
00120             }              
00121             if (Delta1<0)
00122             {
00123                 Delta1=0;
00124             }   
00125             lcd.locate(11,0);
00126             lcd.printf("    ");
00127             lcd.locate(11,0);
00128             lcd.printf("%.1f",Delta1);
00129             wait(0.2);
00130                
00131             if(button3.falling())
00132             {
00133                 j=2;
00134                 led3=0;
00135                 wait(0.3);
00136                 wheel.reset();   
00137             }        
00138         }
00139         if(j==2)
00140         {
00141             Delta2=Delta2+wheel.getPulses();
00142             wheel.reset();
00143             if (Delta2>12000)
00144             {
00145                 Delta2=12000;
00146             }              
00147             if (Delta2<0)
00148             {
00149                 Delta2=0;
00150             }   
00151             lcd.locate(3,1);
00152             lcd.printf("     ");
00153             lcd.locate(3,1);
00154             lcd.printf("%.1f",Delta2);
00155             wait(0.2);
00156             
00157             if(button3.falling())
00158             {
00159                  j=3;
00160                  led3=0;
00161                  wait(0.3);
00162                  wheel.reset();
00163             }    
00164         }
00165         
00166         if(j==3)
00167         {
00168             Delta3=Delta3+wheel.getPulses();
00169             wheel.reset();
00170             if (Delta3>12000)
00171             {
00172                 Delta3=12000;
00173             }              
00174             if (Delta3<0)
00175             {
00176                 Delta3=0;
00177             }   
00178             lcd.locate(11,1);
00179             lcd.printf("     ");
00180             lcd.locate(11,1);
00181             lcd.printf("%.1f",Delta3);
00182             wait(0.2);
00183             
00184             if(button3.falling())
00185             {
00186                  j=0;
00187                  led3=0;
00188                  wait(0.3);
00189                  wheel.reset();
00190             }    
00191         }
00192         
00193         if(j==4)
00194         {
00195             j=0;
00196         }
00197         
00198         if(!button4)
00199         {
00200             break;
00201         }
00202     }
00203  //%%Cierro While--------------------------------------------   
00204     lcd.writeCommand(C4);
00205     lcd.cls(); //borra la pantalla
00206     lcd.printf("  GUARDAMOS \nVALORES |m|"); 
00207     wait(2);
00208     
00209     Delta22=Delta2+Delta1;
00210     Delta33=Delta3+Delta2+Delta1; 
00211     
00212     medd=3.4503686*100*y.read();
00213     lcd.cls();
00214     lcd.locate(0,0);
00215     lcd.printf("Sp:%.1f",Sp);
00216     lcd.locate(0,1);
00217     lcd.printf("Ts=%.1f",T);
00218     //lcd.locate(8,0);
00219     //lcd.printf("Er:%.1f",err);
00220     lcd.locate(8,0);
00221     lcd.printf("T%.1f",medd);
00222     lcd.locate(8,1);
00223     lcd.printf("u:%.1f",PI);
00224     wait(2);
00225     
00226     while(1)
00227     {
00228            
00229            medd=3.45503686*100*y.read();
00230             err = (Sp-medd);
00231             inte=inte+err*Ts;
00232             PI=(kp*err+ ki*inte);
00233             pid=PI/100;
00234             pid2=1-pid;
00235             if(PI>100)
00236                 {
00237                     PI=100;
00238                 }
00239                 if(PI<0)
00240                 {
00241                     PI=0;
00242                 }
00243            
00244            
00245            
00246            
00247            
00248            //Funcion Rampa
00249             if(T<=Delta1)
00250             {
00251             lcd.cls();
00252             lcd.locate(0,0);
00253             Sp=Sp+(((Set-sp)/Delta1))*Ts;
00254             lcd.printf("Sp=%.1f",Sp); 
00255             lcd.locate(0,1);
00256             lcd.printf("Ts=%.1f",T);
00257             lcd.locate(8,0);
00258             lcd.printf("Er:%.1f",medd);
00259             lcd.locate(8,1);
00260             lcd.printf("u:%.1f",PI);
00261             PWMmodule1(pp,pid);
00262             PWMmodule2(pp,pid2);
00263             
00264             }
00265             if((T>Delta1)&&(T<=Delta22))
00266             {
00267             Sp=Sp;
00268             lcd.cls();
00269             lcd.printf("Sp=%.1f",Sp); 
00270             lcd.locate(0,1);
00271             lcd.printf("Ts=%.1f",T);
00272             lcd.locate(8,0);
00273             lcd.printf("Er:%.1f",medd);
00274             lcd.locate(8,1);
00275             lcd.printf("u:%.1f",PI);
00276             PWMmodule1(pp,pid);
00277             PWMmodule2(pp,pid2);
00278             }
00279             if((T>Delta22)&&(T<=Delta33))
00280             {
00281             lcd.cls();
00282             lcd.printf("Sp=%.1f",Sp); 
00283             lcd.locate(0,1);
00284             Sp=Sp-(((Set-sp)/Delta3))*Ts;
00285             lcd.printf("Ts=%.1f",T);
00286             lcd.locate(8,0);
00287             lcd.printf("Er:%.1f",medd);
00288             lcd.locate(8,1);
00289             lcd.printf("u:%.1f",PI);
00290             PWMmodule1(pp,pid);
00291             PWMmodule2(pp,pid2);
00292             
00293             //pid=1-(PI/100);
00294             led=1;
00295             led2=0;
00296           
00297             }
00298             */
00299             PIant=PI;
00300             
00301             lcd.locate(8,0);
00302             lcd.printf("Er:%.1f",medd);
00303             lcd.locate(8,1);
00304             lcd.printf("u:%.1f",PI);
00305             
00306             T=T+Ts; 
00307     }
00308     
00309     
00310 
00311 }