main.cpp

00001 /* Tarea No. 4
00002                 Diego Fernando Carvajal Castrillón
00003                     Santiago Lopez Restrepo
00004                     Ingeniería de Control
00005 
00006 
00007 Practica realizada para aprendizaje de manejo de Encoder. El desarrollo
00008 de esta esta dado por el aumento y disminución de los datos del 
00009 controlador PID, además que al presionar el boton del Encoder suene
00010 mediante un PWM.
00011 */
00012 
00013 
00014 #include "mbed.h"
00015 #include "DebouncedIn.h"
00016 #include "TextLCD.h"
00017 #include "QEI.h"
00018 
00019 
00020 QEI wheel (PTA16, PTA17, NC, 48);
00021 TextLCD lcd(PTB10, PTB11, PTE2, PTE3, PTE4, PTE5); // rs, e, d4-d7
00022 
00023 
00024 DebouncedIn botonEncoder(PTC5);
00025 PwmOut Pwm(PTA5);
00026 
00027  
00028 float periodo=0.001;
00029 float dureza=0.1;
00030 int Spbias=0,Sp=0,Kpbias=0,Kp=0,Ki=0,Kibias=0,Kd=0,Kdbias=0,Enc;
00031 int C1=0x0E; // solo muestra el curzor
00032 int i; // indice de la variable
00033 
00034 int PWMmodule(float pp,float Dd)
00035 {
00036         Pwm.period(periodo);
00037         Pwm.write(dureza);
00038         wait(0.3);
00039         Pwm.write(0); 
00040         return 0;
00041 } 
00042 
00043 int main() 
00044 
00045 {
00046     lcd.writeCommand(C1);
00047     lcd.locate(0,0);
00048     lcd.printf("Sp=0");
00049     lcd.locate(8,0);
00050     lcd.printf("Kp=0");
00051     lcd.locate(0,1);
00052     lcd.printf("Ki=0");
00053     lcd.locate(8,1);
00054     lcd.printf("Kd=0");
00055 
00056     while(1) 
00057     {  
00058      Enc=wheel.getPulses();
00059        
00060                                                 
00061             switch(i) {
00062             
00063                 case 0:
00064                     Kdbias=Kd;
00065                     
00066                     lcd.locate(3,0);
00067                     lcd.printf("%d",Sp);
00068                     
00069                     Sp=Spbias+Enc;  
00070                     if(Sp<0)
00071                     {
00072                         Sp=0; 
00073                         wheel.reset();  
00074                         Enc=0; 
00075                     }                
00076                     break;
00077                 case 1:
00078                     Spbias=Sp;
00079                     lcd.locate(11,0);
00080                     lcd.printf("%d",Kp);
00081                     
00082                     Kp=Kpbias+Enc;  
00083                     if(Kp<0)
00084                     {
00085                         Kp=0; 
00086                         wheel.reset();  
00087                         Enc=0; 
00088                     }  
00089                     break;
00090                 case 2:
00091                     Kpbias=Kp;
00092                     lcd.locate(3,1);
00093                     lcd.printf("%d",Ki);
00094                     
00095                     Ki=Kibias+Enc;  
00096                     if(Ki<0)
00097                     {
00098                         Ki=0; 
00099                         wheel.reset();  
00100                         Enc=0; 
00101                     } 
00102                     
00103                     break;
00104                 case 3:
00105                     Kibias=Ki;
00106                     
00107                     lcd.locate(11,1);
00108                     lcd.printf("%d",Kd);
00109                     Kd=Kdbias+Enc;
00110                     if(Kd<0)
00111                     {
00112                         Kd=0; 
00113                         wheel.reset();  
00114                         Enc=0; 
00115                     } 
00116                     
00117                     break;
00118             }
00119         
00120         
00121                   
00122         
00123         if(botonEncoder.falling()) 
00124         {
00125             PWMmodule(periodo,dureza);
00126             wait(0.2);
00127             wheel.reset();
00128             i++;
00129             if(i>3) 
00130             {
00131                 i=0;
00132             }
00133             switch (i) 
00134             {
00135                 case 0:
00136                     lcd.locate(2,0);
00137                     lcd.printf("=");
00138                     break;
00139                 case 1:
00140                     lcd.locate(10,0);
00141                     lcd.printf("=");
00142                     break;
00143                 case 2:
00144                     lcd.locate(2,1);
00145                     lcd.printf("=");
00146                     break;
00147                 case 3:
00148                     lcd.locate(10,1);
00149                     lcd.printf("=");
00150                     break;
00151                     
00152                     
00153             }
00154         } 
00155     }
00156 }