main.cpp

00001 #include "mbed.h"
00002 #include "TextLCD.h"
00003 #include "DebouncedIn.h"
00004 #include "QEI.h"
00005 
00006 TextLCD lcd(PTC10, PTC11, PTC12, PTC13, PTC16, PTC17); // rs, e, d4-d7.. Nota: No se quiere función de lectura, luego ese puerto a tierra.
00007 //lectura=1, escritura=0
00008 QEI leftQei(PTD6, PTD7, NC, 624);
00009 //Encoder Sp, Kp, Ki, Kd
00010 PwmOut rled(LED_GREEN);
00011 
00012 DebouncedIn next(PTE5);
00013 AnalogIn Ain(PTC2);
00014 AnalogOut Aout(PTE30);
00015 
00016 int C1=0x0F; 
00017 int i,j,k,l;
00018 float ap,ai,ad;
00019 
00020 float p; // set point
00021 float s=0; // salida planta
00022 float h=0; // entrada planta
00023 float m=0; // Error
00024 float mv=0;
00025 float g=0;
00026 float x=0;
00027 
00028 int main() {
00029 
00030 int c;
00031 c=0;
00032 
00033 lcd.cls();     
00034 lcd.locate(0,0);
00035 lcd.printf("sp:%d",i);
00036 lcd.locate(8,0);
00037 lcd.printf("kp:%d",j);
00038 lcd.locate(0,1);
00039 lcd.printf("ki:%d",k);
00040 lcd.locate(8,1);
00041 lcd.printf("kd:%d",l);
00042 
00043 //SP
00044 
00045 while (1){
00046 
00047 if (next.falling()){    
00048         c++;
00049         }          
00050 
00051     //Salto a SP
00052     if (c==0){
00053         
00054         lcd.locate(3,0);
00055         lcd.putc(0xFE);                
00056         lcd.writeCommand(C1);
00057         lcd.locate(3,0);
00058         lcd.printf("%d",i);
00059         
00060             while(c==0){  
00061               
00062             //espacio de trabajo
00063                         
00064                 i=leftQei.getPulses();
00065                 //
00066                 
00067                              
00068                 lcd.locate(3,0);
00069                 lcd.printf("%i",leftQei.getPulses());
00070                 wait(0.2);          
00071                                    
00072            
00073             //fin espacio de trabajo            
00074             
00075             if(next.falling()){
00076                 c++;
00077                 }                                                             
00078                 }
00079                 } 
00080               
00081         
00082     //Salto a Kp
00083     if (c==1){
00084         
00085         lcd.locate(11,0);
00086         lcd.putc(0xFE);                
00087         lcd.writeCommand(C1);
00088         lcd.locate(11,0);
00089         lcd.printf("%d",j);
00090         
00091             while(c==1){
00092             
00093             //Work space
00094             
00095                  j=leftQei.getPulses();
00096                  
00097             //
00098                                              
00099                 lcd.locate(11,0);
00100                 lcd.printf("%i",leftQei.getPulses());
00101                 wait(0.2); 
00102            
00103             //End work space            
00104               
00105              if(next.falling()){
00106                 c++;
00107                 }                                                             
00108                 }
00109                 }
00110               
00111     //Salto a Kp
00112     if (c==2){
00113         
00114         lcd.locate(3,1);
00115         lcd.putc(0xFE);                
00116         lcd.writeCommand(C1);
00117         lcd.locate(3,1);
00118         lcd.printf("%d",k);
00119         
00120             while(c==2){ 
00121             
00122             //Work space
00123             
00124                 k=leftQei.getPulses();
00125                  
00126             //
00127                                              
00128                 lcd.locate(3,1);
00129                 lcd.printf("%i",leftQei.getPulses());
00130                 wait(0.2); 
00131                 
00132             //End work space  
00133 
00134              if(next.falling()){
00135                 c++;
00136                 }                                                             
00137                 }
00138                 }        
00139 
00140     //Salto a Kp
00141     if (c==3){
00142         
00143         lcd.locate(11,1);
00144         lcd.putc(0xFE);                
00145         lcd.writeCommand(C1);
00146         lcd.locate(11,1);
00147         lcd.printf("%d",l);
00148         
00149             while(c==3){
00150             
00151             //Work space
00152             
00153                 l=leftQei.getPulses();
00154                  
00155             //
00156                                              
00157                 lcd.locate(11,1);
00158                 lcd.printf("%i",leftQei.getPulses());
00159                 wait(0.2); 
00160                 
00161            
00162             //End work space                           
00163              
00164              if(next.falling()){
00165                 c++;
00166                                 }                                                             
00167                 }
00168                 }   
00169 
00170     //          
00171                  if(c==4 || c>4){
00172                 c=0;
00173                                 }                                                                                  
00174                          
00175 }
00176 
00177 //Borrar
00178 
00179 lcd.cls();
00180 lcd.printf("Er%d",m);
00181 lcd.locate(8,0);
00182 lcd.printf("Me%d",s);
00183 lcd.locate(0,1);
00184 lcd.printf("Sp%d",i);
00185 lcd.locate(8,1);
00186 lcd.printf("Co%d",x);
00187 wait(5);
00188 
00189 //Ahora empieza el PID
00190 
00191 p=i/100;
00192 
00193 while(1) {
00194         wait(0.3);
00195               
00196         if(p<0.999){
00197        
00198         s=Ain;
00199         m=(p-s);
00200         //n=m*100;
00201         ap=j*m;
00202         ai=(k*m)+ai;
00203         
00204         if (ai>999){
00205         ai=998;
00206                    }
00207         
00208         ad=l*(m-mv);
00209         h=ap+ai+ad;
00210         x=h/100;
00211         g=(h*3.3/100);
00212         
00213         if (h<0){
00214             h=0;
00215                 }
00216                 
00217 
00218                         
00219         Aout=g;
00220         wait(0.22);
00221  
00222         
00223         lcd.cls();
00224         lcd.printf("Er:%.3f",m);
00225         lcd.locate(8,0);
00226         lcd.printf("Me:%.3f",s);
00227         lcd.locate(0,1);
00228         lcd.printf("Sp:%.2f",p);
00229         lcd.locate(8,1);
00230         lcd.printf("Co:%.3f",x);
00231         }
00232 
00233 
00234         mv=m;
00235 
00236         }
00237 }  
00238 
00239 
00240