main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "TextLCD.h"
00004 #include "DebouncedIn.h"
00005 #include "DebounceIn.h"
00006 AnalogIn Vin(PTC2);
00007 AnalogOut Vo(PTE30);
00008 QEI wheel (PTA5, PTD5, NC, 624);
00009 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00010 DebouncedIn b1(PTA13); 
00011 DebounceIn b2(PTC17);
00012 int x=1,i,d,f,g,h=0,m=0,l=0,k=0,j=0;
00013 int C1=0x0e;
00014 float kp, ki, kd, sp ,err, med, yr, pid, ap, ai, ad, err_v ,Sp;
00015 int main()
00016 {
00017 lcd.writeCommand(C1); // escribe el comando C1
00018 lcd.locate(0,0);
00019 lcd.printf("Sp:%d",i);
00020 lcd.locate(8,0);
00021 lcd.printf("Kp:%d",d);
00022 lcd.locate(0,1);
00023 lcd.printf("Ki:%d",f);
00024 lcd.locate(8,1);
00025 lcd.printf("Kd:%d",g);
00026 
00027     while(1) {
00028     j=wheel.getPulses();  
00029      if(b1.falling())
00030      {
00031          x=x+1;
00032      }
00033     if(j<0)
00034     {
00035     wheel.reset();
00036     j=0;
00037     }
00038  
00039 //condicionales para ubicación en pantalla y escritura
00040     if(x==1)
00041     { 
00042     i=j+h;
00043         if(i<0)
00044         {
00045          i=0;
00046         }
00047        wait(0.1);
00048        lcd.locate(3,0);
00049        lcd.printf("    ");   
00050        lcd.locate(3,0);
00051        lcd.printf("%d",i);   
00052     }
00053     if(x==2)
00054     {   
00055     d=(j)-(i)+h+m;
00056          if(d<0)
00057         {
00058             d=0;    
00059         } 
00060          
00061     wait(0.1);
00062     lcd.locate(11,0);
00063     lcd.printf("    ");
00064     lcd.locate(11,0);
00065     lcd.printf("%d",d);          
00066 }
00067     if(x==3)
00068     {
00069     f=j-d-i+h+m+l;
00070         if(f<0)
00071          {
00072             f=0;
00073          }
00074    wait(0.1);
00075    lcd.locate(3,1);
00076    lcd.printf("    ");
00077    lcd.locate(3,1);   
00078     lcd.printf("%d",f);
00079     }
00080    if(x==4)
00081       {
00082        g=j-(i+d+f)+h+m+l+k;
00083           if(g<0)
00084             {
00085             g=0;
00086             }
00087     wait(0.1);
00088     lcd.locate(11,1);
00089     lcd.printf("    ");
00090     lcd.locate(11,1);
00091     lcd.printf("%d",g);
00092     }
00093     if(x>4)
00094     {   
00095         x=1;
00096         h=i;
00097         m=d;
00098         l=f;
00099         k=g;    
00100         wheel.reset();
00101     }
00102     if(b2.read()==0)
00103     {
00104         goto PID;
00105     }
00106 }//while
00107     PID:
00108      
00109         wait(0.5); // espera 1 segundo
00110         lcd.cls();      // borra lcd
00111         wait(0.2);
00112         lcd.printf("DATOS GUARDADOS!");
00113         wait(1);
00114         lcd.cls();
00115       
00116            lcd.locate(0,0);
00117            lcd.printf("Er:");
00118            lcd.locate(8,0);
00119            lcd.printf("Me:");        
00120            lcd.locate(0,1);
00121            lcd.printf("Sp:");
00122            lcd.locate(8,1);
00123            lcd.printf("CO:");  
00124            
00125     while(1)
00126     {//whilePID
00127     wait(0.1);         
00128                    sp=i;
00129                    kp=d;
00130                    kd=f;
00131                    ki=g;
00132            
00133            Sp=(sp*3.3)/100;
00134                med=Vin*3.3;
00135                err = (Sp-med);
00136                ap = kp*err;
00137                ai =(ki*err)+ai;
00138 
00139                ad = kd*(err-err_v);
00140            pid = (ap+ai+ad);
00141              if  (pid<0)
00142            {
00143             pid=0;
00144            }
00145            if (pid>99)
00146            {
00147              pid=100;
00148            }
00149            
00150            Vo=pid/100;
00151                        
00152                                            
00153                                 lcd.locate(3,0);
00154                                 lcd.printf("     ");            
00155            lcd.locate(3,0);
00156            lcd.printf("%.1f",err);
00157                                 lcd.locate(11,0);
00158                                 lcd.printf("     ");            
00159            lcd.locate(11,0);
00160            lcd.printf("%.1f",med);
00161                                 lcd.locate(3,1);
00162                                 lcd.printf("     "); 
00163            lcd.locate(3,1);
00164            lcd.printf("%.1f",Sp);
00165                                 lcd.locate(11,1);
00166                                 lcd.printf("     ");            
00167            lcd.locate(11,1);
00168            lcd.printf("%.1f",pid);
00169            
00170                 err_v = err;
00171     }//whilePID
00172 
00173 
00174 }//main