PID DISCRETO
Dependencies: mbed QEI TextLCD DebouncedIn
main.cpp
00001 // Hello World! for the TextLCD 00002 00003 #include "mbed.h" 00004 #include "stdio.h" 00005 #include "TextLCD.h" 00006 #include "DebouncedIn.h" 00007 #include "QEI.h" 00008 00009 //Declaracion de entradas y salidas 00010 DebouncedIn Boton1(PTE20); //Boton para confirmar 00011 DebouncedIn Boton2(PTE21); //Boton para confirmar 00012 00013 //Configuracion encoder 00014 QEI wheel (PTD5, PTD0, NC, 100); 00015 00016 PwmOut control(PTE29); 00017 AnalogIn vcond(PTB0); 00018 00019 //Configuracion pantalla LCD 00020 TextLCD lcd(PTB8,PTB9,PTB10,PTB11,PTE2,PTE3); // rs, e, d4-d7 00021 00022 //Variables y parametros del control PID 00023 int Kp=0,Ki=0,Kd=0; 00024 float Sp=0; 00025 float K1,K2,K3,ek=0,ekm1=0,ekm2=0,Ts=0.1; //Ts Tiempo Muestreo 00026 float yT,uk=0,ukm1=0; 00027 00028 00029 00030 int main() { 00031 00032 lcd.cls(); 00033 lcd.locate(1,0); 00034 lcd.printf("Kp=%d",Kp); 00035 lcd.locate(9,0); 00036 lcd.printf("Ki%d=",Ki); 00037 lcd.locate(1,1); 00038 lcd.printf("Kd=%d",Kd); 00039 lcd.locate(9,1); 00040 lcd.printf("Sp=%.1f",Sp); 00041 00042 //Configuracion de constante Kp 00043 set_Kp: 00044 lcd.cls(); 00045 lcd.locate(0,0); 00046 lcd.printf(">Kp= "); 00047 00048 lcd.locate(9,0); 00049 lcd.printf("Ki=%d",Ki); 00050 lcd.locate(1,1); 00051 lcd.printf("Kd=%d",Kd); 00052 lcd.locate(9,1); 00053 lcd.printf("Sp=%.1f",Sp); 00054 00055 00056 while(1){ 00057 00058 Kp=Kp+wheel.getPulses(); 00059 wheel.reset(); 00060 00061 if(Kp>=999){ 00062 Kp=999; 00063 } 00064 else if (Kp<=0){ 00065 Kp=0; 00066 } 00067 00068 lcd.locate(0,0); 00069 lcd.printf(">Kp=%d ",Kp); 00070 00071 if(Boton2.falling()){ 00072 goto set_Ki; 00073 } 00074 00075 00076 if(Boton1.falling()){ 00077 goto PID; 00078 } 00079 00080 } 00081 00082 //Configuracion de Ki 00083 set_Ki: 00084 00085 lcd.cls(); 00086 lcd.locate(8,0); 00087 lcd.printf(">Ki= "); 00088 00089 lcd.locate(1,0); 00090 lcd.printf("Kp=%d",Kp); 00091 lcd.locate(1,1); 00092 lcd.printf("Kd=%d",Kd); 00093 lcd.locate(9,1); 00094 lcd.printf("Sp=%.1f",Sp); 00095 00096 while(1){ 00097 00098 Ki=Ki+wheel.getPulses(); 00099 wheel.reset(); 00100 00101 if(Ki>=999){ 00102 Ki=999; 00103 } 00104 else if (Ki<=0){ 00105 Ki=0; 00106 } 00107 00108 lcd.locate(8,0); 00109 lcd.printf(">Ki=%d ",Ki); 00110 00111 if(Boton2.falling()){ 00112 goto set_Kd; 00113 } 00114 00115 if(Boton1.falling()){ 00116 goto PID; 00117 } 00118 00119 } 00120 00121 //Configuracion de Kd 00122 set_Kd: 00123 00124 lcd.cls(); 00125 lcd.locate(0,1); 00126 lcd.printf(">Kd= "); 00127 00128 lcd.locate(1,0); 00129 lcd.printf("Kp=%d",Kp); 00130 lcd.locate(9,0); 00131 lcd.printf("Ki=%d",Ki); 00132 lcd.locate(9,1); 00133 lcd.printf("Sp=%.1f",Sp); 00134 00135 00136 00137 while(1){ 00138 00139 Kd=Kd+wheel.getPulses(); 00140 wheel.reset(); 00141 00142 if(Kd>=999){ 00143 Kd=999; 00144 } 00145 else if (Kd<=0){ 00146 Kd=0; 00147 } 00148 00149 lcd.locate(0,1); 00150 lcd.printf(">Kd=%d ",Kd); 00151 00152 if(Boton2.falling()){ 00153 goto set_Sp; 00154 } 00155 00156 if(Boton1.falling()){ 00157 goto PID; 00158 } 00159 00160 } 00161 00162 //Configuracion de Sp 00163 set_Sp: 00164 00165 lcd.cls(); 00166 lcd.locate(8,1); 00167 lcd.printf(">Sp= "); 00168 00169 lcd.locate(1,0); 00170 lcd.printf("Kp=%d",Kp); 00171 lcd.locate(9,0); 00172 lcd.printf("Ki=%d",Ki); 00173 lcd.locate(1,1); 00174 lcd.printf("Kd=%d",Kd); 00175 00176 00177 while(1){ 00178 00179 Sp=Sp+0.1*wheel.getPulses(); 00180 wheel.reset(); 00181 00182 if(Sp>=3.3){ 00183 Sp=3.3; 00184 } 00185 else if (Sp<=0){ 00186 Sp=0; 00187 } 00188 00189 lcd.locate(8,1); 00190 lcd.printf(">Sp=%.1f ",Sp); 00191 00192 if(Boton2.falling()){ 00193 goto set_Kp; 00194 } 00195 00196 if(Boton1.falling()){ 00197 goto PID; 00198 } 00199 00200 } 00201 00202 //Inicia algoritmo del PID 00203 PID: 00204 00205 lcd.cls(); 00206 while(1){ 00207 00208 yT=vcond.read()*3.3; 00209 ek=Sp-yT; 00210 00211 //Conversion de parametros PID Continuo a parametros de PID Digital 00212 //ESTO ES UN PID DIGITAL Y LOS PARAMETROS AGREGADOS SON PARA UN PID CONTINUO!! 00213 00214 K1=Kp+(Ki/2)*Ts+(Kd/Ts); 00215 K2=-Kp+(Ki/2)*Ts-(2*Kd/Ts); 00216 K3=Kd/Ts; 00217 00218 uk=ukm1+K1*ek+K2*ekm1+K3*ekm2; //Accion de control 00219 00220 if (uk>3.3) { //Salida PID si es mayor que el MAX 00221 uk=3.3;} 00222 else if (uk<0){ //Salida PID si es menor que el MIN 00223 uk=0; 00224 } 00225 00226 control=(float)uk/3.3; //Accion de control mapeada a PWM; salida de la tarjeta. 00227 00228 //Actualizacion de datos para el siguiente paso 00229 ekm2=ekm1; 00230 ekm1=ek; 00231 ukm1=uk; 00232 00233 //Mostrando error y salida actual 00234 lcd.locate(0,0); 00235 lcd.printf("Error=%.2f ",ek); 00236 lcd.locate(0,1); 00237 lcd.printf("Y_act=%.2f",yT); 00238 wait(Ts); //Tiempo de muestreo 00239 } 00240 00241 } 00242 00243
Generated on Sun Jul 17 2022 22:18:41 by 1.7.2